Subversion Repositories Kolibri OS

Rev

Blame | Last modification | View Log | RSS feed

  1. #include "SoC.h"
  2. #include "CPU.h"
  3. #include "MMU.h"
  4. #include "mem.h"
  5. #include "callout_RAM.h"
  6. #include "RAM.h"
  7. #include "cp15.h"
  8. #include "math64.h"
  9. #include "pxa255_IC.h"
  10. #include "pxa255_TIMR.h"
  11. #include "pxa255_RTC.h"
  12. #include "pxa255_UART.h"
  13. #include "pxa255_PwrClk.h"
  14. #include "pxa255_GPIO.h"
  15. #include "pxa255_DMA.h"
  16. #include "pxa255_DSP.h"
  17. #include "pxa255_LCD.h"
  18. #ifdef EMBEDDED
  19.         #include <avr/io.h>
  20. #endif
  21.  
  22.  
  23. #define ERR(s)  do{err_str(s " Halting\r\n"); while(1); }while(0)
  24.  
  25. static const UInt8 embedded_boot[] =    {
  26.                                                 0x01, 0x00, 0x8F, 0xE2, 0x10, 0xFF, 0x2F, 0xE1, 0x04, 0x27, 0x01, 0x20, 0x00, 0x21, 0x00, 0xF0,
  27.                                                 0x0D, 0xF8, 0x0A, 0x24, 0x24, 0x07, 0x65, 0x1C, 0x05, 0x27, 0x00, 0x22, 0x00, 0xF0, 0x06, 0xF8,
  28.                                                 0x20, 0x60, 0x24, 0x1D, 0x49, 0x1C, 0x80, 0x29, 0xF8, 0xD1, 0x28, 0x47, 0xBC, 0x46, 0xBB, 0xBB,
  29.                                                 0x70, 0x47
  30.                                         };
  31.  
  32. #define ROM_BASE        0x00000000UL
  33. #define ROM_SIZE        sizeof(embedded_boot)
  34.  
  35. #define RAM_BASE        0xA0000000UL
  36. #define RAM_SIZE        0x01000000UL    //16M @ 0xA0000000
  37.  
  38.  
  39. static Boolean vMemF(ArmCpu* cpu, void* buf, UInt32 vaddr, UInt8 size, Boolean write, Boolean priviledged, UInt8* fsrP){
  40.        
  41.         SoC* soc = cpu->userData;
  42.         UInt32 pa;
  43.        
  44.         if(size & (size - 1)){  //size is not a power of two
  45.                
  46.                 return false;  
  47.         }
  48.         if(vaddr & (size - 1)){ //bad alignment
  49.                
  50.                 return false;  
  51.         }
  52.  
  53.         return mmuTranslate(&soc->mmu, vaddr, priviledged, write, &pa, fsrP) && memAccess(&soc->mem, pa, size, write, buf);
  54. }
  55.  
  56.  
  57. static Boolean hyperF(ArmCpu* cpu){             //return true if handled
  58.  
  59.         SoC* soc = cpu->userData;
  60.        
  61.         if(cpu->regs[12] == 0){
  62.                 err_str("Hypercall 0 caught\r\n");
  63.                 soc->go = false;
  64.         }
  65.         else if(cpu->regs[12] == 1){
  66.                 err_dec(cpu->regs[0]);
  67.         }
  68.         else if(cpu->regs[12] == 2){
  69.                 char x[2];
  70.                 x[1] = 0;
  71.                 x[0] = cpu->regs[0];
  72.                 err_str(x);
  73.         }
  74.         else if(cpu->regs[12] == 3){                    //get ram size
  75.                 cpu->regs[0] = RAM_SIZE;
  76.         }
  77.         else if(cpu->regs[12] == 4){                    //block device access perform [do a read or write]
  78.                
  79.                 //IN:
  80.                 // R0 = op
  81.                 // R1 = sector
  82.                
  83.                 return soc->blkF(soc->blkD, cpu->regs[1], soc->blkDevBuf, cpu->regs[0]);
  84.         }
  85.         else if(cpu->regs[12] == 5){                    //block device buffer access [read or fill emulator's buffer]
  86.                
  87.                 //IN:
  88.                 // R0 = word value
  89.                 // R1 = word offset (0, 1, 2...)
  90.                 // R2 = op (1 = write, 0 = read)
  91.                 //OUT:
  92.                 // R0 = word value
  93.                
  94.                 if(cpu->regs[1] >= BLK_DEV_BLK_SZ / sizeof(UInt32)) return false;       //invalid request
  95.                
  96.                 if(cpu->regs[2] == 0){
  97.                        
  98.                         cpu->regs[0] = soc->blkDevBuf[cpu->regs[1]];
  99.                 }
  100.                 else if(cpu->regs[2] == 1){
  101.                        
  102.                         soc->blkDevBuf[cpu->regs[1]] = cpu->regs[0];
  103.                 }
  104.                 else return false;
  105.         }
  106.         return true;
  107. }
  108.  
  109. static void setFaultAdrF(ArmCpu* cpu, UInt32 adr, UInt8 faultStatus){
  110.        
  111.         SoC* soc = cpu->userData;
  112.        
  113.         cp15SetFaultStatus(&soc->cp15, adr, faultStatus);
  114. }
  115.  
  116. static void emulErrF(_UNUSED_ ArmCpu* cpu, const char* str){
  117.         err_str("Emulation error: <<");
  118.         err_str(str);
  119.         err_str(">> halting\r\n");
  120.         while(1);
  121. }
  122.  
  123. static Boolean pMemReadF(void* userData, UInt32* buf, UInt32 pa){       //for DMA engine and MMU pagetable walks
  124.  
  125.         ArmMem* mem = userData;
  126.  
  127.         return memAccess(mem, pa, 4, false, buf);
  128. }
  129.  
  130. static void dumpCpuState(ArmCpu* cpu, char* label){
  131.  
  132.         UInt8 i;
  133.  
  134.         if(label){
  135.                 err_str("CPU ");
  136.                 err_str(label);
  137.                 err_str("\r\n");
  138.         }
  139.        
  140.         for(i = 0; i < 16; i++){
  141.                 err_str("R");
  142.                 err_dec(i);
  143.                 err_str("\t= 0x");
  144.                 err_hex(cpuGetRegExternal(cpu, i));
  145.                 err_str("\r\n");       
  146.         }
  147.         err_str("CPSR\t= 0x");
  148.         err_hex(cpuGetRegExternal(cpu, ARM_REG_NUM_CPSR));
  149.         err_str("\r\nSPSR\t= 0x");
  150.         err_hex(cpuGetRegExternal(cpu, ARM_REG_NUM_SPSR));
  151.         err_str("\r\n");
  152. }
  153.  
  154. static UInt16 socUartPrvRead(void* userData){                   //these are special funcs since they always get their own userData - the uart :)
  155.        
  156.         SoC* soc = userData;
  157.         UInt16 v;
  158.         int r;
  159.        
  160.         r = soc->rcF();
  161.         if(r == CHAR_CTL_C) v = UART_CHAR_BREAK;
  162.         else if(r == CHAR_NONE) v = UART_CHAR_NONE;
  163.         else if(r >= 0x100) v = UART_CHAR_NONE;         //we canot send this char!!!
  164.         else v = r;
  165.        
  166.         return v;
  167. }
  168.  
  169. static void socUartPrvWrite(UInt16 chr, void* userData){        //these are special funcs since they always get their own userData - the uart :)
  170.        
  171.         SoC* soc = userData;
  172.        
  173.        
  174.         if(chr == UART_CHAR_NONE) return;
  175.        
  176.         soc->wcF(chr);
  177. }
  178.  
  179. void LinkError_SIZEOF_STRUCT_SOC_wrong();
  180.  
  181.  
  182. void socRamModeAlloc(SoC* soc, _UNUSED_ void* ignored){
  183.        
  184.         UInt32* ramB = emu_alloc(RAM_SIZE);
  185.         if(!ramB) ERR("Cannot allocate RAM buffer");
  186.         if(!ramInit(&soc->ram.RAM, &soc->mem, RAM_BASE, RAM_SIZE, ramB)) ERR("Cannot init RAM");
  187.        
  188.         soc->calloutMem = false;       
  189. }
  190.  
  191. void socRamModeCallout(SoC* soc, void* callout){
  192.        
  193.         if(!coRamInit(&soc->ram.coRAM, &soc->mem, RAM_BASE, RAM_SIZE, callout)) ERR("Cannot init coRAM");
  194.        
  195.         soc->calloutMem = true;
  196. }
  197.  
  198. #define ERR_(s) ERR("error");
  199.  
  200. void socInit(SoC* soc, SocRamAddF raF, void*raD, readcharF rc, writecharF wc, blockOp blkF, void* blkD){
  201.  
  202. printf ("SoC init! \n");
  203.         Err e;
  204.        
  205.         soc->rcF = rc;
  206.         soc->wcF = wc;
  207.        
  208.         soc->blkF = blkF;
  209.         soc->blkD = blkD;
  210.  
  211.         soc->go = true;
  212.        
  213.         e = cpuInit(&soc->cpu, ROM_BASE, vMemF, emulErrF, hyperF, &setFaultAdrF);
  214.         if(e){
  215.                 err_str("Failed to init CPU: ");
  216.         //      err_dec(e);
  217.         //      err_str(". Halting\r\n");
  218.                 while(1);
  219.         }
  220.         printf("CPU init\n");
  221.         soc->cpu.userData = soc;
  222.        
  223.         memInit(&soc->mem);
  224.         mmuInit(&soc->mmu, pMemReadF, &soc->mem);
  225.         printf("Init complete\n");
  226.         if(ROM_SIZE > sizeof(soc->romMem)) {
  227.         //      err_str("Failed to init CPU: ");
  228.                 err_str("ROM_SIZE to small");
  229.         //      err_str(". Halting\r\n");
  230.                 while(1);
  231.         }
  232.        
  233.         printf("RAF\n");
  234.        
  235.         raF(soc, raD);
  236.        
  237.         if(!ramInit(&soc->ROM, &soc->mem, ROM_BASE, ROM_SIZE, soc->romMem)) ERR_("Cannot init ROM");
  238.        
  239.         cp15Init(&soc->cp15, &soc->cpu, &soc->mmu);
  240.        
  241.         __mem_copy(soc->romMem, embedded_boot, sizeof(embedded_boot));
  242.        
  243.         printf("Things...\n");
  244.        
  245.         if(!pxa255icInit(&soc->ic, &soc->cpu, &soc->mem)) ERR_("Cannot init PXA255's interrupt controller");
  246.         if(!pxa255timrInit(&soc->timr, &soc->mem, &soc->ic)) ERR_("Cannot init PXA255's OS timers");
  247.         if(!pxa255rtcInit(&soc->rtc, &soc->mem, &soc->ic)) ERR_("Cannot init PXA255's RTC");
  248.         if(!pxa255uartInit(&soc->ffuart, &soc->mem, &soc->ic,PXA255_FFUART_BASE, PXA255_I_FFUART)) ERR_("Cannot init PXA255's FFUART");
  249.         if(!pxa255uartInit(&soc->btuart, &soc->mem, &soc->ic,PXA255_BTUART_BASE, PXA255_I_BTUART)) ERR_("Cannot init PXA255's BTUART");
  250.         if(!pxa255uartInit(&soc->stuart, &soc->mem, &soc->ic,PXA255_STUART_BASE, PXA255_I_STUART)) ERR_("Cannot init PXA255's STUART");
  251.         if(!pxa255pwrClkInit(&soc->pwrClk, &soc->cpu, &soc->mem)) ERR_("Cannot init PXA255's Power and Clock manager");
  252.         if(!pxa255gpioInit(&soc->gpio, &soc->mem, &soc->ic)) ERR_("Cannot init PXA255's GPIO controller");
  253.         if(!pxa255dmaInit(&soc->dma, &soc->mem, &soc->ic)) ERR_("Cannot init PXA255's DMA controller");
  254.         if(!pxa255dspInit(&soc->dsp, &soc->cpu)) ERR_("Cannot init PXA255's cp0 DSP");
  255.         if(!pxa255lcdInit(&soc->lcd, &soc->mem, &soc->ic)) ERR_("Cannot init PXA255's LCD controller");
  256.  
  257. printf("go?\n");
  258.  
  259.         pxa255uartSetFuncs(&soc->ffuart, socUartPrvRead, socUartPrvWrite, soc);
  260. }
  261.  
  262. void gdbCmdWait(SoC* soc, unsigned gdbPort, int* ss);
  263.  
  264. void socRun(SoC* soc, UInt32 gdbPort){
  265.        
  266.        
  267.         printf("go2?\n");
  268.        
  269.         UInt32 prevRtc = 0;
  270.         UInt32 cyclesCapt = 0;
  271.        
  272.         UInt32 cycles = 0;      //make 64 if you REALLY need it... later
  273.        
  274.         #ifdef GDB_SUPPORT
  275.                 int ss = 1;     //for gdb stub single step
  276.         #else
  277.                 gdbPort = 0;    //use the param somehow to quiet GCC
  278.         #endif
  279.        
  280.         printf("run !\n");
  281.        
  282.         while(soc->go){
  283.                 //printf("Soc go...\n");
  284.                 cycles++;
  285.  
  286.         #ifdef EMBEDDED
  287.                 if(!(PIND & 0x10)){     //btn down
  288.                
  289.                         if(!prevRtc){
  290.                        
  291.                                 do{
  292.                                         prevRtc = gRtc;
  293.                                 }while(prevRtc != gRtc);
  294.                                 cyclesCapt = 0;
  295.                         }
  296.                         else{
  297.                        
  298.                                 UInt32 t;
  299.                                
  300.                                 //we only care to go on if the rtc is now different
  301.                                 do{
  302.                                         t = gRtc;
  303.                                 }while(t != gRtc);
  304.                                
  305.                                 if(t != prevRtc){
  306.                                
  307.                                         if(!cyclesCapt){
  308.                                                
  309.                                                 //this code assumes we're called often enough that the next rtc vals we see is the NEXT second, not the one after or any other such thing
  310.                                                 cyclesCapt = cycles;
  311.                                                 prevRtc = t;
  312.                                         }
  313.                                         else{
  314.                                        
  315.                                                 err_dec(cycles - cyclesCapt);
  316.                                                 err_str(" Hz\r\n");
  317.                                        
  318.                                                 cyclesCapt = 0;
  319.                                                 prevRtc = 0;
  320.                                         }
  321.                                 }
  322.                         }
  323.                 }
  324.         #endif         
  325.                
  326.                 if(!(cycles & 0x00000FUL)) pxa255timrTick(&soc->timr);
  327.                 if(!(cycles & 0x0000FFUL)) pxa255uartProcess(&soc->ffuart);
  328.                 if(!(cycles & 0x000FFFUL)) pxa255rtcUpdate(&soc->rtc);
  329.                 if(!(cycles & 0x01FFFFUL)) pxa255lcdFrame(&soc->lcd);
  330.                
  331.                 #ifdef GDB_SUPPORT
  332.                         gdbCmdWait(soc, gdbPort, &ss);
  333.                 #endif
  334.                
  335.                 cpuCycle(&soc->cpu);
  336.         }
  337. }
  338.  
  339.  
  340.  
  341.  
  342.  
  343.  
  344.  
  345.  
  346.  
  347. #ifdef GDB_SUPPORT
  348.  
  349.  
  350.        
  351.         #include <sys/socket.h>
  352.         #include <sys/time.h>
  353.         #include <sys/types.h>
  354.         #include <sys/select.h>
  355.         #include <unistd.h>
  356.         #include <errno.h>
  357.         #include <stdlib.h>
  358.         #include <netinet/in.h>
  359.         #include <string.h>
  360.         #include <stdio.h>
  361.        
  362.        
  363.        
  364.         static int socdBkptDel(SoC* soc, UInt32 addr, UInt8 sz){
  365.                
  366.                 UInt8 i;
  367.                
  368.                 for(i = 0; i < soc->nBkpt; i++){
  369.                        
  370.                         if(soc->bkpt[i] == addr){
  371.                                
  372.                                 soc->nBkpt--;
  373.                                 soc->bkpt[i] = soc->bkpt[soc->nBkpt];
  374.                                 i--;
  375.                         }      
  376.                 }
  377.                 return 1;
  378.         }
  379.        
  380.        
  381.         static int socdBkptAdd(SoC* soc, UInt32 addr, UInt8 sz){        //boolean
  382.                
  383.                 socdBkptDel(soc, addr, sz);
  384.                
  385.                 if(soc->nBkpt == MAX_BKPT) return 0;
  386.                
  387.                 soc->bkpt[soc->nBkpt++] = addr;
  388.                
  389.                 return 1;
  390.         }
  391.        
  392.         static int socdWtpDel(SoC* soc, UInt32 addr, UInt8 sz){
  393.                
  394.                 UInt8 i;
  395.                
  396.                 for(i = 0; i < soc->nWtp; i++){
  397.                        
  398.                         if(soc->wtpA[i] == addr && soc->wtpS[i] == sz){
  399.                                
  400.                                 soc->nWtp--;
  401.                                 soc->wtpA[i] = soc->wtpA[soc->nWtp];
  402.                                 soc->wtpS[i] = soc->wtpS[soc->nWtp];
  403.                                 i--;
  404.                         }      
  405.                 }
  406.                 return 1;
  407.         }
  408.        
  409.        
  410.         static int socdWtpAdd(SoC* soc, UInt32 addr, UInt8 sz){ //boolean
  411.                
  412.                 socdWtpDel(soc, addr, sz);
  413.                
  414.                 if(soc->nWtp == MAX_WTP) return 0;
  415.                
  416.                 soc->wtpA[soc->nWtp] = addr;
  417.                 soc->wtpS[soc->nWtp] = sz;
  418.                 soc->nWtp++;
  419.                
  420.                 return 1;
  421.         }
  422.        
  423.        
  424.         UInt32 htoi(const char** cP){
  425.                
  426.                 UInt32 i = 0;
  427.                 const char* in = *cP;
  428.                 char c;
  429.                
  430.                 while((c = *in) != 0){
  431.                        
  432.                         if(c >= '0' && c <= '9') i = (i * 16) + (c - '0');
  433.                         else if(c >= 'a' && c <= 'f') i = (i * 16) + (c + 10 - 'a');
  434.                         else if(c >= 'A' && c <= 'F') i = (i * 16) + (c + 10 - 'A');
  435.                         else break;
  436.                         in++;
  437.                 }
  438.                
  439.                 *cP = in;
  440.                
  441.                 return i;
  442.         }
  443.        
  444.         static UInt32 swap32(UInt32 x){
  445.                
  446.                 return ((x >> 24) & 0xff) | ((x >> 8) & 0xff00) | ((x & 0xff00) << 8) | ((x & 0xff) << 24);    
  447.         }
  448.        
  449.        
  450.         int gdb_memAccess(SoC* soc, UInt32 addr, UInt8* buf, int write){
  451.                
  452.                 UInt32 pa = 0;
  453.                 UInt8 fsr = 0;
  454.                
  455.                 return mmuTranslate(&soc->mmu, addr, true, false, &pa, &fsr) && memAccess(&soc->mem, pa, 1, write | 0x80, buf);
  456.         }
  457.        
  458.         static int addRegToStr(SoC* soc, char* str, int reg){
  459.                
  460.                 if(reg == 0x19 || reg < 0x10){
  461.                         if(reg == 0x19) reg = ARM_REG_NUM_CPSR;
  462.                         sprintf(str + strlen(str), "%08x", swap32(cpuGetRegExternal(&soc->cpu, reg)));
  463.                 }
  464.                 else if(reg >= 0x10 && reg < 0x18){
  465.                        
  466.                         strcat(str, "000000000000000000000000");
  467.                 }
  468.                 else if(reg == 0x18){   //fps
  469.                        
  470.                         strcat(str, "00000000");
  471.                 }
  472.                 else return 0;
  473.                
  474.                 return 1;
  475.         }
  476.        
  477.         static int interpPacket(SoC* soc,const char* in, char* out, int* ss){   //return 0 if we failed to interp a command, 1 is all ok, -1 to send no reply and run
  478.                
  479.                 ArmCpu* cpu = &soc->cpu;
  480.                 unsigned char c;
  481.                 unsigned addr, len;
  482.                 unsigned char* ptr;
  483.                 int i;
  484.                 int ret = 1;
  485.                
  486.                
  487.                
  488.                 if(strcmp(in, "qSupported") == 0){
  489.                        
  490.                         strcpy(out, "PacketSize=99");  
  491.                 }
  492.                 else if(strcmp(in, "vCont?") == 0){
  493.                        
  494.                         out[0] = 0;
  495.                 }
  496.                 else if(strcmp(in, "s") == 0){          //single step
  497.                        
  498.                         *ss = 1;
  499.                         return -1;
  500.                 }
  501.                 else if(strcmp(in, "c") == 0 || in[0] == 'C'){          //continue [with signal, which we ignore]
  502.                        
  503.                         return -1;
  504.                 }
  505.                 else if(in[0] == 'Z' || in[0] == 'z'){
  506.                        
  507.                         char op = in[0];
  508.                         char type = in[1];
  509.                         int (*f)(SoC* soc, UInt32 addr, UInt8 sz) = NULL;
  510.                        
  511.                         in += 3;
  512.                         addr = htoi(&in);
  513.                         if(*in++ != ',') goto fail;     //no comma?
  514.                         len = htoi(&in);
  515.                         if(*in) goto fail;              //string not over?
  516.                        
  517.                         if(type == '0' || type == '1'){ //bkpt
  518.                                
  519.                                 f = (op == 'Z') ? socdBkptAdd : socdBkptDel;
  520.                         }
  521.         /*
  522.                         else if(type == '2' || type == '3'){    //wtp
  523.                                
  524.                                 f = (op == 'Z') ? socdWtpAdd : socdWtpDel;
  525.                         }
  526.                         else goto fail;
  527.         */
  528.                         strcpy(out,f(soc, addr, len) ? "OK" : "e00");
  529.                 }
  530.                 else if(in[0] == 'H' && (in[1] == 'c' || in[1] == 'g')){
  531.                         strcpy(out, "OK");     
  532.                 }
  533.                 else if(in[0] == 'q'){
  534.                        
  535.                         if(in[1] == 'C'){
  536.                                
  537.                                 strcpy(out, "");       
  538.                         }
  539.                         else if(strcmp(in  +1, "Offsets") == 0){
  540.                                
  541.                                 strcpy(out, "Text=0;Data=0;Bss=0");
  542.                         }
  543.                         else goto fail;
  544.                 }
  545.                 else if(in[0] == 'p'){  //read register
  546.                        
  547.                         in++;
  548.                         i = htoi(&in);
  549.                         if(*in) goto fail;      //string not over?
  550.                        
  551.                         out[0] = 0;
  552.                         if(!addRegToStr(soc, out, i)) goto fail;
  553.                 }
  554.                 else if(strcmp(in, "g") == 0){  //read all registers
  555.                        
  556.                         out[0] = 0;
  557.                         for(i = 0; i < 0x1a; i++) if(!addRegToStr(soc, out, i)) goto fail;
  558.                 }
  559.                 else if(in[0] == 'P'){  //write register
  560.                        
  561.                         in++;
  562.                         i = htoi(&in);
  563.                         if(*in++ != '=') goto fail;     //string not over?
  564.                         if(i == 0x19 || i <16){
  565.                                 if(i == 0x19) i = ARM_REG_NUM_CPSR;
  566.                                 addr = htoi(&in);
  567.                                 sprintf(out, "OK");
  568.                                 cpuSetReg(cpu, i, addr);
  569.                         }
  570.                         else strcpy(out,"e00");
  571.                 }
  572.                 else if(in[0] == 'm'){  //read memory
  573.                        
  574.                         in++;
  575.                         addr = htoi(&in);
  576.                         if(*in++ != ',') goto fail;
  577.                         len = htoi(&in);
  578.                         if(*in) goto fail;
  579.                         out[0] = 0;
  580.                         while(len--){
  581.                                
  582.                                 if(!gdb_memAccess(soc, addr++, &c, false)) break;
  583.                                 sprintf(out + strlen(out), "%02x", c); 
  584.                         }
  585.                 }
  586.                 else if(strcmp(in, "?") == 0){
  587.                        
  588.                         strcpy(out,"S05");     
  589.                 }
  590.                 else goto fail;
  591.                
  592.         send_pkt:
  593.                 return ret;
  594.                
  595.         fail:
  596.                 out[0] = 0;
  597.                 ret = 0;
  598.                 goto send_pkt;
  599.         }
  600.        
  601.         static void sendpacket(int sock, char* packet, int withAck){
  602.                
  603.                 unsigned int c;
  604.                 int i;
  605.                                
  606.                 c = 0;
  607.                 for(i = 0; i < strlen(packet); i++) c += packet[i];
  608.                 memmove(packet + (withAck ? 2 : 1), packet, strlen(packet) + 1);
  609.                 if(withAck){
  610.                         packet[0] = '+';
  611.                         packet[1] = '$';
  612.                 }
  613.                 else{
  614.                         packet[0] = '$';
  615.                 }
  616.                 sprintf(packet + strlen(packet), "#%02x", c & 0xFF);
  617.                
  618.                 //printf("sending packet <<%s>>\n", packet);
  619.                 send(sock, packet, strlen(packet), 0); 
  620.         }
  621.        
  622.         void gdbCmdWait(SoC* soc, unsigned gdbPort, int* ss){
  623.                
  624.                 ArmCpu* cpu = &soc->cpu;
  625.                 static int running = 0;
  626.                 static int sock = -1;
  627.                 char packet[4096];
  628.                 struct timeval tv = {0};
  629.                 fd_set set;
  630.                 int ret;
  631.                
  632.                 if(*ss && running){
  633.                        
  634.                         strcpy(packet,"S05");
  635.                         sendpacket(sock, packet, 0);
  636.                         running = 0;    //perform single step
  637.                 }
  638.                 *ss = 0;
  639.                
  640.                 if(running){    //check for breakpoints
  641.                        
  642.                         UInt8 i;
  643.                        
  644.                         for(i = 0; i < soc->nBkpt; i++){
  645.                                
  646.                                 if(soc->cpu.regs[15] == soc->bkpt[i]){
  647.                                        
  648.                                 //      printf("bkpt hit: pc=0x%08lX bk=0x%08lX i=%d\n", soc->cpu.regs[15], soc->bkpt[i], i);
  649.                                         strcpy(packet,"S05");
  650.                                         sendpacket(sock, packet, 0);
  651.                                         running = 0;    //perform breakpoint hit
  652.                                         break;
  653.                                 }
  654.                         }
  655.                 }
  656.                
  657.                 if(gdbPort){
  658.                        
  659.                         if(sock == -1){ //no socket yet - make one
  660.                                
  661.                                 struct sockaddr_in sa = {AF_INET, htons(gdbPort)};
  662.                                 socklen_t sl = sizeof(sa);
  663.                                
  664.                                 inet_aton("127.0.0.1", &sa.sin_addr.s_addr);
  665.                                
  666.                                 sock = socket(PF_INET, SOCK_STREAM, 0);
  667.                                 if(sock == -1){
  668.                                         err_str("gdb socket creation fails: ");
  669.                                         err_dec(errno);
  670.                                         ERR("\n");
  671.                                 }
  672.                                
  673.                                 ret = bind(sock, (struct sockaddr*)&sa, sizeof(sa));
  674.                                 if(ret){
  675.                                         err_str("gdb socket bind fails: ");
  676.                                         err_dec(errno);
  677.                                         ERR("\n");
  678.                                 }
  679.                                
  680.                                 ret = listen(sock, 1);
  681.                                 if(ret){
  682.                                         err_str("gdb socket listen fails: ");
  683.                                         err_dec(errno);
  684.                                         ERR("\n");
  685.                                 }
  686.                                
  687.                                 ret = accept(sock, (struct sockaddr*)&sa, &sl);
  688.                                 if(ret == -1){
  689.                                         err_str("gdb socket accept fails: ");
  690.                                         err_dec(errno);
  691.                                         ERR("\n");
  692.                                 }
  693.                                 close(sock);
  694.                                 sock = ret;
  695.                                
  696.                                 soc->nBkpt = 0;
  697.                                 soc->nWtp = 0;
  698.                         }
  699.                 }
  700.                 if(gdbPort){
  701.                                
  702.                         do{
  703.                
  704.                                 FD_ZERO(&set);
  705.                                 FD_SET(sock, &set);
  706.                                 tv.tv_sec = running ? 0 : 0x00f00000UL;
  707.                                 do{
  708.                                         ret = select(sock + 1, &set, NULL, NULL, &tv);
  709.                                 }while(!ret && !running);
  710.                                 if(ret < 0){
  711.                                         err_str("select fails: ");
  712.                                         err_dec(errno);
  713.                                         ERR("\n");
  714.                                 }
  715.                                 if(ret > 0){
  716.                                         char c;
  717.                                         char* p;
  718.                                         int i, len = 0, esc = 0, end = 0;
  719.                                        
  720.                                         ret = recv(sock, &c, 1, 0);
  721.                                         if(ret != 1) ERR("failed to receive byte (1)\n");
  722.                                        
  723.                                         if(c == 3){
  724.                                                 strcpy(packet,"S11");
  725.                                                 sendpacket(sock, packet, 0);
  726.                                                 running = 0;    //perform breakpoint hit
  727.                                         }
  728.                                         else if(c != '$'){
  729.                                                 //printf("unknown packet header '%c'\n", c);
  730.                                         }
  731.                                         else{
  732.                                                 do{
  733.                                                         if(esc){
  734.                                                                 c = c ^ 0x20;
  735.                                                                 esc = 0;
  736.                                                         }
  737.                                                         else if(c == 0x7d){
  738.                                                                 esc = 1;
  739.                                                         }
  740.                                                        
  741.                                                         if(!esc){       //we cannot be here if we're being escaped
  742.                                                                
  743.                                                                 packet[len++] = c;
  744.                                                                 if(end == 0 && c == '#') end = 2;
  745.                                                                 else if(end){
  746.                                                                        
  747.                                                                         end--;
  748.                                                                         if(!end) break;
  749.                                                                 }
  750.                                                                
  751.                                                                 ret = recv(sock, &c, 1, 0);
  752.                                                                 if(ret != 1) ERR("failed to receive byte (2)\n");
  753.                                                         }
  754.                                                 }while(1);
  755.                                                 packet[len] = 0;
  756.                                                
  757.                                                 memmove(packet, packet + 1, len);
  758.                                                 len -= 4;
  759.                                                 packet[len] = 0;
  760.                                                 ret = interpPacket(soc, p = strdup(packet), packet, ss);
  761.                                                 if(ret == 0) printf("how do i respond to packet <<%s>>\n", p);
  762.                                                 if(ret == -1){  //ack it anyways
  763.                                                         char c = '+';
  764.                                                         send(sock, &c, 1, 0);
  765.                                                         running = 1;
  766.                                                 }
  767.                                                 else sendpacket(sock, packet, 1);
  768.                                                
  769.                                                 emu_free(p);
  770.                                         }
  771.                                 }
  772.                         }while(!running);
  773.                 }
  774.         }
  775.  
  776. #endif
  777.