swo.c (4305B)
1 /* swo.c 2 * 3 * Copyright 2015 Brian Swetland <swetland@frotz.net> 4 * 5 * Licensed under the Apache License, Version 2.0 (the "License"); 6 * you may not use this file except in compliance with the License. 7 * You may obtain a copy of the License at 8 * 9 * http://www.apache.org/licenses/LICENSE-2.0 10 * 11 * Unless required by applicable law or agreed to in writing, software 12 * distributed under the License is distributed on an "AS IS" BASIS, 13 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 14 * See the License for the specific language governing permissions and 15 * limitations under the License. 16 */ 17 18 #include <stdio.h> 19 #include <stdlib.h> 20 #include <unistd.h> 21 #include <string.h> 22 23 #include <pthread.h> 24 #include <fw/types.h> 25 #include <debugger.h> 26 27 #define TRACE 0 28 29 static char console_data[256]; 30 static int console_ptr = 0; 31 32 static void handle_swv_src(unsigned id, unsigned val, unsigned n) { 33 #if TRACE 34 xprintf(XDATA, "SRC %s %02x %08x\n", (id & 0x100) ? "HW" : "SW", id & 0xFF, val); 35 #endif 36 if (id == 0) { 37 console_data[console_ptr++] = val; 38 if (val == '\n') { 39 console_data[console_ptr] = 0; 40 xprintf(XDATA, "[remote] %s", console_data); 41 console_ptr = 0; 42 } else if (console_ptr == 254) { 43 console_data[console_ptr] = 0; 44 xprintf(XDATA, "[remote] %s\n", console_data); 45 } 46 } 47 } 48 49 static void handle_swv_proto(unsigned char *data, unsigned len) { 50 #if TRACE 51 switch (len) { 52 case 1: 53 xprintf(XDATA, "PRO %02x\n", data[0]); 54 break; 55 case 2: 56 xprintf(XDATA, "PRO %02x %02x\n", data[0], data[1]); 57 break; 58 case 3: 59 xprintf(XDATA, "PRO %02x %02x %02x\n", 60 data[0], data[1], data[2]); 61 break; 62 case 4: 63 xprintf(XDATA, "PRO %02x %02x %02x %02x\n", 64 data[0], data[1], data[2], data[3]); 65 break; 66 case 5: 67 xprintf(XDATA, "PRO %02x %02x %02x %02x %02x\n", 68 data[0], data[1], data[2], data[3], data[4]);; 69 break; 70 case 6: 71 xprintf(XDATA, "PRO %02x %02x %02x %02x %02x %02x\n", 72 data[0], data[1], data[2], data[3], 73 data[4], data[5]); 74 break; 75 case 7: 76 xprintf(XDATA, "PRO %02x %02x %02x %02x %02x %02x %02x\n", 77 data[0], data[1], data[2], data[3], 78 data[4], data[5], data[6]); 79 break; 80 } 81 #endif 82 } 83 84 typedef enum { 85 SWV_SYNC, 86 SWV_1X1, 87 SWV_1X2, 88 SWV_1X4, 89 SWV_2X2, 90 SWV_2X4, 91 SWV_3X4, 92 SWV_4X4, 93 SWV_PROTO, 94 SWV_IDLE, 95 } swv_state_t; 96 97 typedef struct { 98 swv_state_t state; 99 unsigned zcount; 100 unsigned ccount; 101 unsigned id; 102 unsigned val; 103 unsigned char data[8]; 104 } swv_t; 105 106 static swv_t swv = { 107 .state = SWV_SYNC, 108 .zcount = 0 109 }; 110 111 static void handle_swv(swv_t *swv, unsigned x) { 112 // any sequence ending in 00 00 00 00 00 80 is a re-sync 113 if (x == 0) { 114 swv->zcount++; 115 } else { 116 if ((swv->zcount >= 5) && (x == 0x80)) { 117 swv->state = SWV_IDLE; 118 swv->zcount = 0; 119 #if TRACE 120 xprintf(XDATA, "SYNC\n"); 121 #endif 122 return; 123 } 124 swv->zcount = 0; 125 } 126 127 switch (swv->state) { 128 case SWV_IDLE: 129 if (x & 7) { 130 // AAAAAHSS source packet 131 swv->id = (x >> 3) | ((x & 4) << 6); 132 swv->val = 0; 133 swv->state = (x & 3); 134 } else if (x != 0) { 135 // CXXXXX00 protocol packet 136 swv->data[0] = x; 137 if (x & 0x80) { 138 swv->ccount = 1; 139 swv->state = SWV_PROTO; 140 } else { 141 handle_swv_proto(swv->data, 1); 142 } 143 } else { 144 // 00 packets are for sync, ignore 145 } 146 break; 147 case SWV_PROTO: 148 swv->data[swv->ccount++] = x; 149 // protocol packets end at 7 total bytes or a byte with bit7 clear 150 if ((swv->ccount == 7) || (!(x & 0x80))) { 151 handle_swv_proto(swv->data, swv->ccount); 152 swv->state = SWV_IDLE; 153 } 154 break; 155 case SWV_1X1: 156 handle_swv_src(swv->id, x, 1); 157 swv->state = SWV_IDLE; 158 break; 159 case SWV_1X2: 160 swv->val = x; 161 swv->state = SWV_2X2; 162 break; 163 case SWV_2X2: 164 handle_swv_src(swv->id, swv->val | (x << 8), 2); 165 swv->state = SWV_IDLE; 166 break; 167 case SWV_1X4: 168 swv->val = x; 169 swv->state = SWV_2X4; 170 break; 171 case SWV_2X4: 172 swv->val |= (x << 8); 173 swv->state = SWV_3X4; 174 break; 175 case SWV_3X4: 176 swv->val |= (x << 16); 177 swv->state = SWV_4X4; 178 break; 179 case SWV_4X4: 180 handle_swv_src(swv->id, swv->val | (x << 24), 4); 181 swv->state = SWV_IDLE; 182 break; 183 case SWV_SYNC: 184 break; 185 default: 186 // impossible 187 xprintf(XDATA, "fatal error, bad state %d\n", swv->state); 188 exit(1); 189 } 190 } 191 192 void process_swo_data(void *_data, unsigned count) { 193 unsigned char *data = _data; 194 while(count-- > 0) { 195 handle_swv(&swv, *data++); 196 } 197 } 198