swdp.c (3920B)
1 /* swdp.c 2 * 3 * Copyright 2011 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 <fw/types.h> 19 #include <fw/io.h> 20 #include <fw/lib.h> 21 22 #include <arch/hardware.h> 23 #include <protocol/rswdp.h> 24 25 volatile unsigned count; 26 27 unsigned data[8]; 28 29 static inline void DIR_OUT(unsigned n) { 30 writel(SSP_CR0_BITS(n) | SSP_CR0_FRAME_SPI | 31 SSP_CR0_CLK_HIGH | SSP_CR0_PHASE1 | 32 SSP_CR0_CLOCK_RATE(1), 33 SSP0_CR0); 34 writel(IOCON_FUNC_1 | IOCON_DIGITAL, IOCON_PIO0_9); /* MOSI */ 35 } 36 37 static inline void DIR_IN(unsigned n) { 38 writel(IOCON_FUNC_0 | IOCON_DIGITAL, IOCON_PIO0_9); /* MOSI */ 39 writel(SSP_CR0_BITS(n) | SSP_CR0_FRAME_SPI | 40 SSP_CR0_CLK_HIGH | SSP_CR0_PHASE0 | 41 SSP_CR0_CLOCK_RATE(1), 42 SSP0_CR0); 43 } 44 45 static inline void XMIT(unsigned n) { 46 writel(n, SSP0_DR); 47 while (readl(SSP0_SR) & SSP_BUSY); 48 readl(SSP0_DR); 49 } 50 51 static inline unsigned RECV(void) { 52 writel(0, SSP0_DR); 53 while (readl(SSP0_SR) & SSP_BUSY); 54 return readl(SSP0_DR); 55 } 56 57 void swdp_reset(void) { 58 /* clock out 64 1s */ 59 DIR_OUT(16); 60 writel(0xFFFF, SSP0_DR); 61 writel(0xFFFF, SSP0_DR); 62 writel(0xFFFF, SSP0_DR); 63 writel(0xFFFF, SSP0_DR); 64 while (readl(SSP0_SR) & SSP_BUSY) ; 65 readl(SSP0_DR); 66 readl(SSP0_DR); 67 readl(SSP0_DR); 68 readl(SSP0_DR); 69 70 /* clock out 16bit init sequence */ 71 writel(0b0111100111100111, SSP0_DR); 72 while (readl(SSP0_SR) & SSP_BUSY) ; 73 readl(SSP0_DR); 74 75 /* clock out 64 1s */ 76 writel(0xFFFF, SSP0_DR); 77 writel(0xFFFF, SSP0_DR); 78 writel(0xFFFF, SSP0_DR); 79 writel(0xFFFF, SSP0_DR); 80 while (readl(SSP0_SR) & SSP_BUSY) ; 81 readl(SSP0_DR); 82 readl(SSP0_DR); 83 readl(SSP0_DR); 84 readl(SSP0_DR); 85 } 86 87 static unsigned revbits(unsigned n) { 88 n = ((n >> 1) & 0x55555555) | ((n << 1) & 0xaaaaaaaa); 89 n = ((n >> 2) & 0x33333333) | ((n << 2) & 0xcccccccc); 90 n = ((n >> 4) & 0x0f0f0f0f) | ((n << 4) & 0xf0f0f0f0); 91 n = ((n >> 8) & 0x00ff00ff) | ((n << 8) & 0xff00ff00); 92 n = ((n >> 16) & 0x0000ffff) | ((n << 16) & 0xffff0000); 93 return n; 94 } 95 96 /* returns 1 if the number of bits set in n is odd */ 97 static unsigned parity(unsigned n) { 98 n = (n & 0x55555555) + ((n & 0xaaaaaaaa) >> 1); 99 n = (n & 0x33333333) + ((n & 0xcccccccc) >> 2); 100 n = (n & 0x0f0f0f0f) + ((n & 0xf0f0f0f0) >> 4); 101 n = (n & 0x00ff00ff) + ((n & 0xff00ff00) >> 8); 102 n = (n & 0x0000ffff) + ((n & 0xffff0000) >> 16); 103 return n & 1; 104 } 105 106 volatile int count_rd_wait = 0; 107 volatile int count_wr_wait = 0; 108 109 int swdp_write(unsigned n, unsigned v) { 110 unsigned a, p; 111 112 again: 113 /* clock out 8 0s and read sequence */ 114 XMIT(n); 115 116 /* read X R0 R1 R2 */ 117 DIR_IN(4); 118 a = RECV(); 119 if ((a & 7) != 4) { 120 DIR_OUT(16); 121 if ((a & 7) == 2) { 122 count_wr_wait++; 123 goto again; 124 } 125 return -1; 126 } 127 128 p = parity(v); 129 v = revbits(v); 130 131 /* transmit X D0..D31 P */ 132 DIR_OUT(9); 133 XMIT(0x100 | (v >> 24)); 134 DIR_OUT(16); 135 XMIT(v >> 8); 136 DIR_OUT(9); 137 XMIT((v << 1) | p); 138 139 DIR_OUT(16); 140 141 return 0; 142 } 143 144 int swdp_read(unsigned n, unsigned *out) { 145 unsigned a, b, c; 146 147 again: 148 /* clock out 8 0s and read sequence */ 149 XMIT(n); 150 151 /* read X R0 R1 R2 */ 152 DIR_IN(4); 153 a = RECV(); 154 if ((a & 7) != 4) { 155 DIR_OUT(16); 156 if ((a & 7) == 2) { 157 count_rd_wait++; 158 goto again; 159 } 160 *out = 0xffffffff; 161 return -1; 162 } 163 164 /* D0..D31 P X */ 165 DIR_IN(16); 166 a = RECV(); 167 DIR_IN(8); 168 b = RECV(); 169 DIR_IN(10); 170 c = RECV(); 171 172 *out = revbits((a << 16) | (b << 8) | (c >> 2)); 173 DIR_OUT(16); 174 return 0; 175 }