aboutsummaryrefslogtreecommitdiff
path: root/AT91SAM7S256/armdebug/Debugger/debug_opcodes.S
blob: 307da8bd65d132397ad5a288e358afdbcac0047f (plain)
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
358
359
360
361
362
363
364
365
366
367
368
369
370
371
372
373
374
375
376
377
378
379
380
381
382
383
384
385
386
387
388
389
390
391
392
393
394
395
396
397
398
399
400
401
402
403
404
405
406
407
408
409
410
411
412
413
414
415
416
417
418
419
420
421
422
423
424
425
426
427
428
429
430
431
432
433
434
435
436
437
438
439
440
441
442
443
444
445
446
447
448
449
450
451
452
453
454
455
456
457
458
459
460
461
462
463
464
465
466
467
468
469
470
471
472
473
474
475
476
477
478
479
480
481
482
483
484
485
486
487
488
489
490
491
492
493
494
495
496
497
498
499
500
501
502
503
504
505
506
507
508
509
510
511
512
513
514
515
516
517
518
519
520
521
522
523
524
525
526
527
528
529
530
531
532
533
534
535
536
537
538
539
540
541
542
543
544
545
546
547
548
549
550
551
552
553
554
555
556
557
558
559
560
561
562
563
564
565
566
567
568
569
570
571
572
573
574
575
576
577
578
579
580
581
582
583
584
585
586
587
588
589
590
591
592
593
594
595
596
597
598
599
600
601
602
603
604
605
606
607
608
609
610
611
612
613
614
615
616
617
618
619
620
621
622
623
624
625
626
627
628
629
630
631
632
633
634
635
636
637
638
639
640
641
642
643
644
645
646
647
648
649
650
651
652
653
654
655
656
657
658
659
660
661
662
663
664
665
666
667
668
669
670
671
672
673
674
675
676
677
678
679
680
681
682
683
684
685
686
687
688
689
690
691
692
693
694
695
696
697
698
699
700
701
702
703
704
705
706
707
708
709
710
711
712
713
714
715
716
717
718
719
720
721
722
723
724
725
726
727
728
729
730
731
732
733
734
735
736
737
738
739
740
741
742
743
744
745
746
747
748
749
750
751
752
753
754
755
756
757
758
759
760
761
762
763
764
765
766
767
768
769
770
771
772
773
774
775
776
777
778
779
780
781
782
783
784
785
786
787
788
789
790
791
792
793
794
795
796
797
798
799
800
801
802
803
804
805
806
807
808
809
810
811
812
813
814
815
816
817
818
819
820
821
822
823
824
825
826
827
828
829
830
831
832
833
834
835
836
837
838
839
840
841
842
843
844
845
846
847
848
849
850
851
852
853
854
855
856
857
858
859
860
861
862
863
864
865
866
867
868
869
870
871
872
873
874
875
876
877
878
879
880
881
882
883
884
885
886
887
888
889
890
891
892
893
894
895
896
897
898
899
900
901
902
903
904
905
906
907
908
909
910
911
912
913
914
915
916
917
918
919
920
921
922
923
924
925
926
927
928
929
930
931
932
933
934
935
936
937
938
939
940
941
942
943
944
945
946
947
948
949
950
951
952
953
954
955
956
957
958
959
960
961
962
963
964
965
966
967
968
969
970
971
972
973
974
975
976
977
978
979
980
981
982
983
984
985
986
987
988
989
990
991
992
993
994
995
996
997
998
999
1000
1001
1002
1003
1004
1005
1006
1007
1008
1009
1010
1011
1012
1013
1014
1015
1016
1017
1018
1019
1020
1021
1022
1023
1024
1025
1026
1027
1028
1029
1030
1031
1032
1033
1034
1035
1036
1037
1038
1039
1040
1041
1042
1043
1044
1045
1046
1047
1048
1049
1050
1051
1052
1053
1054
1055
1056
1057
1058
1059
1060
1061
1062
1063
1064
1065
1066
1067
1068
1069
1070
1071
1072
1073
1074
1075
1076
1077
1078
1079
1080
1081
1082
1083
1084
1085
1086
1087
1088
1089
1090
1091
1092
1093
1094
1095
1096
1097
1098
1099
1100
1101
1102
1103
1104
1105
1106
1107
1108
1109
1110
1111
1112
1113
1114
1115
1116
1117
1118
1119
1120
1121
1122
1123
1124
1125
1126
1127
1128
1129
1130
1131
1132
1133
1134
1135
1136
1137
1138
1139
1140
1141
1142
1143
1144
1145
1146
1147
1148
1149
1150
1151
1152
1153
1154
1155
1156
1157
1158
1159
1160
1161
1162
1163
1164
1165
1166
1167
1168
1169
1170
1171
1172
1173
1174
1175
1176
1177
1178
1179
1180
1181
1182
1183
1184
1185
1186
1187
1188
1189
1190
1191
1192
1193
1194
1195
1196
1197
1198
1199
1200
1201
1202
1203
1204
1205
1206
1207
1208
1209
1210
1211
1212
1213
1214
1215
1216
1217
1218
1219
1220
1221
1222
1223
1224
1225
1226
1227
1228
1229
1230
1231
1232
1233
1234
1235
1236
1237
1238
1239
1240
1241
1242
1243
1244
1245
1246
1247
1248
1249
1250
1251
1252
1253
1254
1255
1256
1257
1258
1259
1260
1261
1262
1263
1264
1265
1266
1267
1268
1269
1270
1271
1272
1273
1274
1275
1276
1277
1278
1279
1280
1281
1282
1283
1284
1285
1286
1287
1288
1289
1290
1291
1292
1293
1294
1295
1296
1297
1298
1299
1300
1301
1302
1303
1304
1305
1306
1307
1308
1309
1310
1311
1312
1313
1314
1315
1316
1317
1318
1319
1320
1321
1322
1323
1324
1325
1326
1327
1328
1329
1330
1331
1332
1333
1334
1335
1336
1337
1338
1339
1340
1341
1342
1343
1344
1345
1346
1347
1348
1349
1350
1351
1352
1353
1354
1355
1356
1357
1358
1359
1360
1361
1362
1363
1364
1365
1366
1367
1368
1369
1370
1371
1372
1373
1374
1375
1376
1377
1378
1379
1380
1381
1382
1383
1384
1385
1386
1387
1388
1389
1390
1391
1392
1393
1394
1395
1396
1397
1398
1399
1400
1401
1402
1403
1404
1405
1406
1407
1408
1409
1410
1411
1412
1413
1414
1415
1416
1417
1418
1419
1420
1421
1422
1423
1424
1425
1426
1427
1428
1429
1430
1431
1432
1433
1434
1435
1436
1437
1438
1439
1440
1441
1442
1443
1444
1445
1446
1447
1448
1449
1450
1451
1452
1453
1454
1455
1456
1457
1458
1459
1460
1461
1462
1463
1464
1465
1466
1467
1468
1469
1470
1471
1472
1473
1474
1475
1476
1477
1478
1479
1480
1481
1482
1483
1484
1485
1486
1487
1488
1489
1490
1491
1492
1493
1494
1495
1496
1497
1498
1499
/** @file debug_opcodes.S
 *  @brief ARM Debugger Opcode Parsing Routines
 *
 */

/* Copyright (C) 2007-2011 the NxOS developers
 *
 * Module Developed by: TC Wan <tcwan@cs.usm.my>
 *
 * See AUTHORS for a full list of the developers.
 *
 * See COPYING for redistribution license
 *
 */

/* WARNING: The following excepted code from eCos arm_stub.c has bugs in
 * the next instruction address calculation logic. The C code has not been
 * updated since it is only used for documentation purposes.
 *
 * Correct code behavior should be determined from the ARMDEBUG source code
 * whenever there is conflict in the algorithms.
 *
 * Of note: ARM and Thumb mode BX PC handling (missing PC+8/PC+4 adjustment).
 *          LDM PC handling (missing Pre/Post Incr/Decr adjustment).
 */
/****************************************************************************
// Selected Routines from the eCos arm_stub.c related to next instruction address
// determination in ARM processors.

//========================================================================
//
//      arm_stub.c
//
//      Helper functions for stub, generic to all ARM processors
//
//========================================================================
// ####ECOSGPLCOPYRIGHTBEGIN####
// -------------------------------------------
// This file is part of eCos, the Embedded Configurable Operating System.
// Copyright (C) 1998, 1999, 2000, 2001, 2002, 2003 Free Software Foundation, Inc.
//
// eCos is free software; you can redistribute it and/or modify it under
// the terms of the GNU General Public License as published by the Free
// Software Foundation; either version 2 or (at your option) any later
// version.
//
// eCos is distributed in the hope that it will be useful, but WITHOUT
// ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
// FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License
// for more details.
//
// You should have received a copy of the GNU General Public License
// along with eCos; if not, write to the Free Software Foundation, Inc.,
// 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
//
// As a special exception, if other files instantiate templates or use
// macros or inline functions from this file, or you compile this file
// and link it with other works to produce a work based on this file,
// this file does not by itself cause the resulting work to be covered by
// the GNU General Public License. However the source code for this file
// must still be made available in accordance with section (3) of the GNU
// General Public License v2.
//
// This exception does not invalidate any other reasons why a work based
// on this file might be covered by the GNU General Public License.
// -------------------------------------------
// ####ECOSGPLCOPYRIGHTEND####
//========================================================================
//#####DESCRIPTIONBEGIN####
//
// Author(s):     Red Hat, gthomas
// Contributors:  Red Hat, gthomas, jskov
// Date:          1998-11-26
// Purpose:
// Description:   Helper functions for stub, generic to all ARM processors
// Usage:
//
//####DESCRIPTIONEND####
//
//========================================================================


static int
ins_will_execute(unsigned long ins)
{
    unsigned long psr = get_register(PS);  // condition codes
    int res = 0;
    switch ((ins & 0xF0000000) >> 28) {
    case 0x0: // EQ
        res = (psr & PS_Z) != 0;
        break;
    case 0x1: // NE
        res = (psr & PS_Z) == 0;
        break;
    case 0x2: // CS
        res = (psr & PS_C) != 0;
        break;
    case 0x3: // CC
        res = (psr & PS_C) == 0;
        break;
    case 0x4: // MI
        res = (psr & PS_N) != 0;
        break;
    case 0x5: // PL
        res = (psr & PS_N) == 0;
        break;
    case 0x6: // VS
        res = (psr & PS_V) != 0;
        break;
    case 0x7: // VC
        res = (psr & PS_V) == 0;
        break;
    case 0x8: // HI
        res = ((psr & PS_C) != 0) && ((psr & PS_Z) == 0);
        break;
    case 0x9: // LS
        res = ((psr & PS_C) == 0) || ((psr & PS_Z) != 0);
        break;
    case 0xA: // GE
        res = ((psr & (PS_N|PS_V)) == (PS_N|PS_V)) ||
            ((psr & (PS_N|PS_V)) == 0);
        break;
    case 0xB: // LT
        res = ((psr & (PS_N|PS_V)) == PS_N) ||
            ((psr & (PS_N|PS_V)) == PS_V);
        break;
    case 0xC: // GT
        res = ((psr & (PS_N|PS_V)) == (PS_N|PS_V)) ||
            ((psr & (PS_N|PS_V)) == 0);
        res = ((psr & PS_Z) == 0) && res;
        break;
    case 0xD: // LE
        res = ((psr & (PS_N|PS_V)) == PS_N) ||
            ((psr & (PS_N|PS_V)) == PS_V);
        res = ((psr & PS_Z) == PS_Z) || res;
        break;
    case 0xE: // AL
        res = TRUE;
        break;
    case 0xF: // NV
        if (((ins & 0x0E000000) >> 24) == 0xA)
	    res = TRUE;
	else
	    res = FALSE;
        break;
    }
    return res;
}

static unsigned long
RmShifted(int shift)
{
    unsigned long Rm = get_register(shift & 0x00F);
    int shift_count;
    if ((shift & 0x010) == 0) {
        shift_count = (shift & 0xF80) >> 7;
    } else {
        shift_count = get_register((shift & 0xF00) >> 8);
    }
    switch ((shift & 0x060) >> 5) {
    case 0x0: // Logical left
        Rm <<= shift_count;
        break;
    case 0x1: // Logical right
        Rm >>= shift_count;
        break;
    case 0x2: // Arithmetic right
        Rm = (unsigned long)((long)Rm >> shift_count);
        break;
    case 0x3: // Rotate right
        if (shift_count == 0) {
            // Special case, RORx
            Rm >>= 1;
            if (get_register(PS) & PS_C) Rm |= 0x80000000;
        } else {
            Rm = (Rm >> shift_count) | (Rm << (32-shift_count));
        }
        break;
    }
    return Rm;
}

// Decide the next instruction to be executed for a given instruction
static unsigned long *
target_ins(unsigned long *pc, unsigned long ins)
{
    unsigned long new_pc, offset, op2;
    unsigned long Rn;
    int i, reg_count, c;

    switch ((ins & 0x0C000000) >> 26) {
    case 0x0:
        // BX or BLX
        if ((ins & 0x0FFFFFD0) == 0x012FFF10) {
            new_pc = (unsigned long)get_register(ins & 0x0000000F);
            return ((unsigned long *)new_pc);
        }
        // Data processing
        new_pc = (unsigned long)(pc+1);
        if ((ins & 0x0000F000) == 0x0000F000) {
            // Destination register is PC
            if ((ins & 0x0FBF0000) != 0x010F0000) {
                Rn = (unsigned long)get_register((ins & 0x000F0000) >> 16);
                if ((ins & 0x000F0000) == 0x000F0000) Rn += 8;  // PC prefetch!
                if ((ins & 0x02000000) == 0) {
                    op2 = RmShifted(ins & 0x00000FFF);
                } else {
                    op2 = ins & 0x000000FF;
                    i = (ins & 0x00000F00) >> 8;  // Rotate count
                    op2 = (op2 >> (i*2)) | (op2 << (32-(i*2)));
                }
                switch ((ins & 0x01E00000) >> 21) {
                case 0x0: // AND
                    new_pc = Rn & op2;
                    break;
                case 0x1: // EOR
                    new_pc = Rn ^ op2;
                    break;
                case 0x2: // SUB
                    new_pc = Rn - op2;
                    break;
                case 0x3: // RSB
                    new_pc = op2 - Rn;
                    break;
                case 0x4: // ADD
                    new_pc = Rn + op2;
                    break;
                case 0x5: // ADC
                    c = (get_register(PS) & PS_C) != 0;
                    new_pc = Rn + op2 + c;
                    break;
                case 0x6: // SBC
                    c = (get_register(PS) & PS_C) != 0;
                    new_pc = Rn - op2 + c - 1;
                    break;
                case 0x7: // RSC
                    c = (get_register(PS) & PS_C) != 0;
                    new_pc = op2 - Rn +c - 1;
                    break;
                case 0x8: // TST
                case 0x9: // TEQ
                case 0xA: // CMP
                case 0xB: // CMN
                    break; // PC doesn't change
                case 0xC: // ORR
                    new_pc = Rn | op2;
                    break;
                case 0xD: // MOV
                    new_pc = op2;
                    break;
                case 0xE: // BIC
                    new_pc = Rn & ~op2;
                    break;
                case 0xF: // MVN
                    new_pc = ~op2;
                    break;
                }
            }
        }
        return ((unsigned long *)new_pc);
    case 0x1:
        if ((ins & 0x02000010) == 0x02000010) {
            // Undefined!
            return (pc+1);
        } else {
            if ((ins & 0x00100000) == 0) {
                // STR
                return (pc+1);
            } else {
                // LDR
                if ((ins & 0x0000F000) != 0x0000F000) {
                    // Rd not PC
                    return (pc+1);
                } else {
                    Rn = (unsigned long)get_register((ins & 0x000F0000) >> 16);
                    if ((ins & 0x000F0000) == 0x000F0000) Rn += 8;  // PC prefetch!
                    if (ins & 0x01000000) {
                        // Add/subtract offset before
                        if ((ins & 0x02000000) == 0) {
                            // Immediate offset
                            if (ins & 0x00800000) {
                                // Add offset
                                Rn += (ins & 0x00000FFF);
                            } else {
                                // Subtract offset
                                Rn -= (ins & 0x00000FFF);
                            }
                        } else {
                            // Offset is in a register
                            if (ins & 0x00800000) {
                                // Add offset
                                Rn += RmShifted(ins & 0x00000FFF);
                            } else {
                                // Subtract offset
                                Rn -= RmShifted(ins & 0x00000FFF);
                            }
                        }
                    }
                    return ((unsigned long *)*(unsigned long *)Rn);
                }
            }
        }
        return (pc+1);
    case 0x2:  // Branch, LDM/STM
        if ((ins & 0x02000000) == 0) {
            // LDM/STM
            if ((ins & 0x00100000) == 0) {
                // STM
                return (pc+1);
            } else {
                // LDM
                if ((ins & 0x00008000) == 0) {
                    // PC not in list
                    return (pc+1);
                } else {
                    Rn = (unsigned long)get_register((ins & 0x000F0000) >> 16);
                    if ((ins & 0x000F0000) == 0x000F0000) Rn += 8;  // PC prefetch!
                    offset = ins & 0x0000FFFF;
                    reg_count = 0;
                    for (i = 0;  i < 15;  i++) {
                        if (offset & (1<<i)) reg_count++;
                    }
                    if (ins & 0x00800000) {
                        // Add offset
                        Rn += reg_count*4;
                    } else {
                        // Subtract offset
                        Rn -= 4;
                    }
                    return ((unsigned long *)*(unsigned long *)Rn);
                }
            }
        } else {
            // Branch
            if (ins_will_execute(ins)) {
                offset = (ins & 0x00FFFFFF) << 2;
                if (ins & 0x00800000) offset |= 0xFC000000;  // sign extend
                new_pc = (unsigned long)(pc+2) + offset;
		// If its BLX, make new_pc a thumb address.
		if ((ins & 0xFE000000) == 0xFA000000) {
		    if ((ins & 0x01000000) == 0x01000000)
			new_pc |= 2;
		    new_pc = MAKE_THUMB_ADDR(new_pc);
		}
                return ((unsigned long *)new_pc);
            } else {
                // Falls through
                return (pc+1);
            }
        }
    case 0x3:  // Coprocessor & SWI
        if (((ins & 0x03000000) == 0x03000000) && ins_will_execute(ins)) {
           // SWI
           return (unsigned long *)(CYGNUM_HAL_VECTOR_SOFTWARE_INTERRUPT * 4);
        } else {
           return (pc+1);
        }
    default:
        // Never reached - but fixes compiler warning.
        return 0;
    }
}

// FIXME: target_ins also needs to check for CPSR/THUMB being set and
//        set the thumb bit accordingly.

static unsigned long
target_thumb_ins(unsigned long pc, unsigned short ins)
{
    unsigned long new_pc = MAKE_THUMB_ADDR(pc+2); // default is fall-through
                                        // to next thumb instruction
    unsigned long offset, arm_ins, sp;
    int i;

    switch ((ins & 0xf000) >> 12) {
    case 0x4:
        // Check for BX or BLX
        if ((ins & 0xff07) == 0x4700)
            new_pc = (unsigned long)get_register((ins & 0x00078) >> 3);
        break;
    case 0xb:
        // push/pop
        // Look for "pop {...,pc}"
        if ((ins & 0xf00) == 0xd00) {
            // find PC
            sp = (unsigned long)get_register(SP);

            for (offset = i = 0; i < 8; i++)
              if (ins & (1 << i))
                  offset += 4;

            new_pc = *(cyg_uint32 *)(sp + offset);

            if (!v5T_semantics())
                new_pc = MAKE_THUMB_ADDR(new_pc);
        }
        break;
    case 0xd:
        // Bcc | SWI
        // Use ARM function to check condition
        arm_ins = ((unsigned long)(ins & 0x0f00)) << 20;
        if ((arm_ins & 0xF0000000) == 0xF0000000) {
            // SWI
            new_pc = CYGNUM_HAL_VECTOR_SOFTWARE_INTERRUPT * 4;
        } else if (ins_will_execute(arm_ins)) {
            offset = (ins & 0x00FF) << 1;
            if (ins & 0x0080) offset |= 0xFFFFFE00;  // sign extend
            new_pc = MAKE_THUMB_ADDR((unsigned long)(pc+4) + offset);
        }
        break;
    case 0xe:
        // check for B
        if ((ins & 0x0800) == 0) {
            offset = (ins & 0x07FF) << 1;
            if (ins & 0x0400) offset |= 0xFFFFF800;  // sign extend
            new_pc = MAKE_THUMB_ADDR((unsigned long)(pc+4) + offset);
        }
        break;
    case 0xf:
        // BL/BLX (4byte instruction!)
        // First instruction (bit 11 == 0) holds top-part of offset
        if ((ins & 0x0800) == 0) {
	    offset = (ins & 0x07FF) << 12;
	    if (ins & 0x0400) offset |= 0xFF800000;  // sign extend
	    // Get second instruction
	    // Second instruction (bit 11 == 1) holds bottom-part of offset
	    ins = *(unsigned short*)(pc+2);
	    // Check for BL/BLX
	    if ((ins & 0xE800) == 0xE800) {
		offset |= (ins & 0x07ff) << 1;
		new_pc = (unsigned long)(pc+4) + offset;
		// If its BLX, force a full word alignment
		// Otherwise, its a thumb address.
		if (!(ins & 0x1000))
		    new_pc &= ~3;
		else
		    new_pc = MAKE_THUMB_ADDR(new_pc);
	    }
	}
        break;
    }

    return new_pc;
}

void __single_step (void)
{
    unsigned long pc = get_register(PC);
    unsigned long cpsr = get_register(PS);

    // Calculate address of next instruction to be executed
    if (cpsr & CPSR_THUMB_ENABLE) {
        // thumb
        ss_saved_pc = target_thumb_ins(pc, *(unsigned short*)pc);
    } else {
        // ARM
        unsigned long curins = *(unsigned long*)pc;
        if (ins_will_execute(curins)) {
            // Decode instruction to decide what the next PC will be
            ss_saved_pc = (unsigned long) target_ins((unsigned long*)pc,
                                                     curins);
        } else {
            // The current instruction will not execute (the conditions
            // don't hold)
            ss_saved_pc = pc+4;
        }
    }

    // Set breakpoint according to type
    if (IS_THUMB_ADDR(ss_saved_pc)) {
        // Thumb instruction
        unsigned long t_pc = UNMAKE_THUMB_ADDR(ss_saved_pc);
        ss_saved_thumb_instr = *(unsigned short*)t_pc;
        *(unsigned short*)t_pc = HAL_BREAKINST_THUMB;
    } else {
        // ARM instruction
        ss_saved_instr = *(unsigned long*)ss_saved_pc;
        *(unsigned long*)ss_saved_pc = HAL_BREAKINST_ARM;
    }
}

 ****************************************************************************/

#define __ASSEMBLY__
#include "debug_stub.h"
#include "debug_internals.h"
#include "debug_macros.h"

.data
.align 4
/* Rm Shifted Shift Type Jump Table
 * On entry:
 *      R0: Register Rm
 *      R1: Shift/Rotate Amount
 * On exit:
 *      R0: RmShifted result
 *
 */
debug_regShiftJumpTable:
    .word   _reg_lsl                            /* 00 */
    .word   _reg_lsr                            /* 01 */
    .word   _reg_asr                            /* 02 */
    .word   _reg_ror                            /* 03 */
    .word   _reg_rrx                            /* 04 */

/* Data Processing Instruction Jump Table
 * On entry:
 *      R0: Register Rn (Operand 1) value
 *      R1: Operand 2 value
 *      R2: Default Next Instruction Address
 *      R5[3:0]: CPSR condition codes
 * On exit:
 *      R0: Calculated result
 *      R1, R2, R3: Destroyed
 *
 */
debug_dataInstrJumpTable:
    .word   _opcode_and                         /* 00 */
    .word   _opcode_eor                         /* 01 */
    .word   _opcode_sub                         /* 02 */
    .word   _opcode_rsb                         /* 03 */
    .word   _opcode_add                         /* 04 */
    .word   _opcode_adc                         /* 05 */
    .word   _opcode_sbc                         /* 06 */
    .word   _opcode_rsc                         /* 07 */
    .word   _opcode_tst                         /* 08 */
    .word   _opcode_teq                         /* 09 */
    .word   _opcode_cmp                         /* 0A */
    .word   _opcode_cmn                         /* 0B */
    .word   _opcode_orr                         /* 0C */
    .word   _opcode_mov                         /* 0D */
    .word   _opcode_bic                         /* 0E */
    .word   _opcode_mvn                         /* 0F */


/*
 * To determine the next instruction to execute, we need to check current (breakpointed) instruction
 * and determine whether it will be executed or not. This necessitates a mini instruction decoder
 * that can check the type of instruction, as well as if it'll affect the PC.
 * The instruction decoder used here is table based. Each entry in the table consists of:
 *		Instruction Identifier (IID), Instruction Bitmask (IBM), Instruction Handler Address (IHA)
 * Null entries are placed at the end of the table.
 *
 * This allows for a flexible approach to handling instructions that we're interested in, at the expense
 * of memory usage.
 *
 * For ARM, the IID & IBM are both 4 bytes, whereas the Thumb IID & IBM are 2 bytes.
 * The IHA is always 4 bytes.
 */

/* ARM Instruction Decode Table
 *	.word IID, IBM, IHA (12 bytes)
 */

/* WARNING: The sequence of matching instructions is important!
 *			Always check from more specific to more general IBMs
 *			for instructions sharing common opcode prefix bits.
 */
debug_armDecodeTable:
	.word	0x012fff10, 0x0ffffff0, _arm_bx_blx_handler		/* [Prefix:00] BX or BLX. Note v4t does not have BLX instr */
	.word	0x0000f000, 0x0c00f000, _arm_data_instr_handler	/* [Prefix:00] Data Processing instr with Rd = R15 */
/*	.word	0x06000010, 0x0e000010, _arm_undef_handler	*/	/* [Prefix:01] Undefined instr: shouldn't occur, as it would've been trapped already. See _dbg_following_instruction_addr */
	.word	0x0410f000, 0x0410f000, _arm_ldr_pc_handler		/* [Prefix:01] LDR with Rd = PC */
	.word	0x08108000, 0x0e108000, _arm_ldm_pc_handler		/* [Prefix:10] LDM {pc} */
	.word	0x0a000000, 0x0e000000, _arm_b_bl_blx_handler	/* [Prefix:10] B, BL or BLX. Note v4t does not have BLX instr */
	.word	0x0c000000, 0x0c000000, _arm_coproc_swi_handler	/* [Prefix:11] Coprocessor instr or SWI */
	.word	0x0,0x0,0x0										/* Null Entry */

/* Thumb Instruction Decode Table
 * 	.hword IID, IBM
 * 	.word IHA (8 bytes)
 */

/* WARNING: The sequence of matching instructions is important!
 *			Always check from more specific to more general IBMs
 *			for instructions sharing common opcode prefix bits.
 */
debug_thumbDecodeTable:
	.hword	0x4700, 0xff07
	.word	_thumb_bx_blx_handler				/* [Prefix:01] BX or BLX. Note: Link (L:b7) is not checked in the mask */
	.hword	0xbd00, 0xff00
	.word	_thumb_poppc_handler				/* [Prefix:10] PUSH/POP, specifically POP {Rlist,PC} */
	.hword	0xd000, 0xf000
	.word	_thumb_bcond_swi_handler			/* [Prefix:11] B<cond> or SWI */
	.hword	0xe000, 0xf800
	.word	_thumb_b_handler					/* [Prefix:11] B */
	.hword	0xf000, 0xf000
	.word	_thumb_long_bl_blx_handler			/* [Prefix:11] Long BL or BLX (4 bytes) Note: b11 (H) indicates 1st or 2nd instr */
	.hword	0x0,0x0
	.word	0x0									/* Null Entry */

/* ARM Condition Code Mapping Table
 * Converts Instruction encoding to SPSR Flags.
 * b31 b30 b29 b28
 *  N   Z   C   V
 * Indexed according to Instruction Encoding order (pg 30, Table 6, ATMEL ARM7TDMI Data Sheet)
 * Condition Code stored in MSN(set), LSN(clr) order
 * Note1: 0x00 = AL. NV is deprecated, treat as AL
 * Note2: 0xFF indicates that the condition checks needs to be handled separately (complex checks)
 *
 *		EQ: Z set
 *		NE: Z clr
 *		HS/CS: C set
 *		LO/CC: C clr
 *		MI: N set
 *		PL: N clr
 *		VS: V set
 *		VC: V clr
 */


debug_armCondCodeTable:
			/* EQ,   NE, HS/CS, LO/CC,   MI,   PL,   VS,   VC,   HI,   LS,   GE,   LT,   GT,   LE,   AL,   NV */
	.byte	 0x40, 0x04,  0x20,  0x02, 0x80, 0x08, 0x10, 0x01, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0x00, 0x00

/* ARM Complex Condition Code Mapping Table
 * Converts Instruction encoding to SPSR Flags.
 * b31 b30 b29 b28
 *  N   Z   C   V
 * Indexed according to Instruction Encoding order (pg 30, Table 6, ATMEL ARM7TDMI Data Sheet)
 * for HI, LS, GE, LT, GT and LE instructions only
 * Condition Code stored in the following order:
 *	 b7   b6   b5    b4   b3    b2     b1    b0
 *	AND  CHKZ CHKC CHKNV   -  Z set  C set  N==V		(bit set = 1)
 *   OR    -    -     -    -  Z clr  C clr  N!=V		(bit clr = 0)
 *
 *		HI: C set AND Z clr
 *		LS: C clr  OR Z set
 *		GE: N == V
 *		LT: N != V
 *		GT: Z clr AND (N == V)
 *		LE: Z set OR (N != V)
 */

#define COMPLEX_CONDCODE_START	    0x08
#define COMPLEX_CONDCODE_NEQV_MASK  0x01
#define COMPLEX_CONDCODE_CSET_MASK  0x02
#define COMPLEX_CONDCODE_ZSET_MASK  0x04
#define COMPLEX_CONDCODE_CHKNV_MASK 0x10
#define COMPLEX_CONDCODE_CHKC_MASK  0x20
#define COMPLEX_CONDCODE_CHKZ_MASK  0x40
#define COMPLEX_CONDCODE_ANDOR_MASK 0x80

#define COMPLEX_CONDCODE_NFLAG		0x08
#define COMPLEX_CONDCODE_ZFLAG		0x04
#define COMPLEX_CONDCODE_CFLAG		0x02
#define COMPLEX_CONDCODE_VFLAG		0x01


debug_armComplexCCTable:
			/* HI,   LS,   GE,   LT,   GT,   LE */
	.byte	 0xE2, 0x64, 0x11, 0x10, 0xD1, 0x54

.code 32
.text
.align 	4

/* dbg_following_instruction_addr
 *		Determine the address of the following instruction to execute.
 *      On entry:
 *          R0: Address of the instruction to be (re)executed
 *		On exit:
 *          R0: Destroyed
 *			R1: Following Instruction Address (31 bits, b0 = THUMB flag)
 *          R2-R7: Destroyed
 *
 *	Here we make use of the Debugger Stack which contains the address of the aborted instruction that will be reexecuted
 *	when we resume the program.
 *
 *	If it is a Manual Breakpoint inserted into the code, then we will need to update the aborted instruction
 *	address to skip the current aborted instruction and resume execution at the next instruction address,
 *  and the next instruction address to be returned to the calling routine is the following instruction
 *  address instead.
 *
 *	We need to check the aborted instruction type, to see if it is a branch instruction, before we can determine
 *	the next instruction address (for inserting a Breakpoint).
 */
	.global dbg_following_instruction_addr
dbg_following_instruction_addr:
    stmfd   sp!, {lr}
/* We assume that any BKPT instructions in the code will be Manual Breakpoints,
 * i.e., the Debugger does not leave stray Single Step / Auto / Normal breakpoints in memory
 */
    mov     r6, r0                                      /* Keep instruction address in R6 */
    _getdbgregister DBGSTACK_USERCPSR_INDEX, r1         /* Retrieve User CPSR into R1 */
    and     r0, r1, #CPSR_THUMB                         /* store Thumb Mode status in R0 */
    mov     r5, r1, lsr #28                             /* store CPSR condition flags in R5[3:0] */

_dbg_get_aborted_instr:
1:  teq     r0, #0                                      /* Check if it is ARM or Thumb instruction */
    ldrneh  r4, [r6]                                    /* Load Thumb instruction opcode using Addr in R6 into R4 */
    ldrne   r2, =(BKPT16_INSTR | BKPT16_MANUAL_BKPT)    /* check for Thumb Manual Breakpoint Instruction */
    ldreq   r4, [r6]                                    /* Load ARM instruction opcode using Addr in R6 into R4 */
    ldreq   r2, =(BKPT32_INSTR | BKPT32_MANUAL_BKPT)    /* check for ARM Manual Breakpoint Instruction */
    teq     r4, r2                                      /* Is instruction opcode (R4) == Manual Breakpoint opcode (R2)? */
    bne     2f                                          /* Not Manual breakpoint */
    teq     r0, #0                                      /* Check if it is ARM or Thumb Manual Breakpoint */
    addne   r6, r6, #2                                  /* Is Manual Breakpoint, Skip to next Thumb instruction */
    addeq   r6, r6, #4                                  /* Is Manual Breakpoint, Skip to next ARM instruction */
    b       1b                                          /* To protect against a sequence of Manual Breakpoint Instructions */

/* Here, R4 contains the instruction opcode which will be (re)executed when program resumes.
 * We need to dissect it to see if it is a branch instruction.
 * For ARM instructions, we also need to evaluate the current (breakpointed) instruction to see if it'll execute.
 * If not, then the following instruction is at the address following the address of the opcode in R4 (Default Following Instruction Address in R6).
 */
2:
    teq     r0, #0                                      /* Check if current instruction is ARM or Thumb instruction */
    beq     _following_instr_addr_for_arm
_following_instr_addr_for_thumb:
    add     r6, r6, #2                                  /* Store default following Thumb instruction address to R6 */
#if 0
	/* Flag Thumb instruction only within the instruction handler */
    orr     r6, r6, #BKPT_STATE_THUMB_FLAG              /* Set b0 to indicate Thumb instruction */
#endif
    /* R4: Candidate Instruction Opcode
     * R5[3:0]: CPSR condition codes
     * R6: Default Following Instruction Address (PC+2)
     */
    bl      _eval_thumb_instruction						/* following address is either ARM or Thumb */
    /* We must set this the Thumb bit only within the instruction handler since BX would switch modes */
    b       _exit_dbg_following_instruction_addr

_following_instr_addr_for_arm:
    add     r6, r6, #4                                  /* Store default following ARM instruction address to R6 */
    /* R4: Candidate Instruction Opcode
     * R5[3:0]: CPSR condition codes
     * R6: Default Following Instruction Address (PC+4)
     */
    bl      _eval_arm_instruction

_exit_dbg_following_instruction_addr:
    mov     r1, r0                                      /* Return Actual Following Instruction Address in R1 (B0 set to indicate Thumb mode) */
    ldmfd   sp!, {pc}


/* _eval_arm_instruction
 *      Evaluate ARM instruction to determine following instruction address
 *      On entry:
 *          R4: Opcode of instruction to be executed
 *          R5[3:0]: CPSR condition codes
 *          R6: Default Following Instruction Address (PC+4)
 *      On exit:
 *          R0: following instruction address (B0 set to indicate Thumb mode)
 *          R1-R7: destroyed
 */
_eval_arm_instruction:
    stmfd   sp!, {lr}
    bl      _dbg_check_arm_condcode		/* Returns R0: will_execute (boolean) */
    teq     r0, #FALSE
    moveq   r0, r6                      /* If False (don't execute), so use Default Following Instruction Address */
    beq     _exit_eval_arm_instruction	/* and Return to caller */

_will_execute_arm_instr:
    mov     r0, #0                      /* initialize ARM Decode Entry Table index register */
1:
    _dbg_armDecodeEntry r1, r2, r3, r0  /* instrreg (R1), instrmask (R2), codehandler (R3), indexreg (R0) */
    teq     r1, #0                      /* Check for Null Entry (End of Table marker) */
    moveq   r0, r6                      /* End of Table, no match found, so use Default Following Instruction Address */
    beq     _exit_eval_arm_instruction
    and     r7, r4, r2                  /* Use R7 to check masked instruction opcode (from R4) to see if it matches template (in R1) */
    teq     r7, r1
    addne   r0, r0, #1                  /* No match, so keep looking */
    bne     1b

_call_arm_code_handler:
    mov     lr, pc
    bx      r3                          /* Call Code Handler with R4: Instruction Opcode, R5[3:0]: CPSR, R6: Default Following Instruction Address */
_exit_eval_arm_instruction:
    /* Returned Following Address Instruction in R0 (B0 set to indicate Thumb mode)  */
    ldmfd   sp!, {pc}

/* _eval_thumb_instruction
 *      Evaluate Thumb instruction to determine following instruction address
 *      On entry:
 *          R4: Opcode of instruction to be executed
 *          R5[3:0]: CPSR condition codes
 *          R6: Default Following Instruction Address (PC+2)
 *      On exit:
 *          R0: following instruction address (B0 set to indicate Thumb mode)
 *          R1-R7: destroyed
 */
_eval_thumb_instruction:
    stmfd   sp!, {lr}
	/* Only B<cond> instructions are conditionally executed, deal with it in that Code Handler */
    mov     r0, #0                      /* initialize Thumb Decode Entry Table index register */
1:
    _dbg_thumbDecodeEntry r1, r2, r3, r0  /* instrreg (R1), instrmask (R2), codehandler (R3), indexreg (R0) */
    teq     r1, #0                      /* Check for Null Entry (End of Table marker) */
    moveq   r0, r6                      /* End of Table, no match found, so use Default Following Instruction Address */
    orreq  	r0, r0, #BKPT_STATE_THUMB_FLAG  /* Set R0[0] to flag Thumb mode */
    beq     _exit_eval_thumb_instruction

    and     r7, r4, r2                  /* Use R5 to check masked instruction opcode (from R4) to see if it matches template (in R1) */
    teq     r7, r1
    addne   r0, r0, #1                  /* No match, so keep looking */
    bne     1b

_call_thumb_code_handler:
    mov     lr, pc
    bx      r3                          /* Call Code Handler with R4: Instruction Opcode, R5[3:0]: CPSR, R6: Default Following Instruction Address */
_exit_eval_thumb_instruction:
    /* Returned Following Address Instruction in R0 */
    ldmfd   sp!, {pc}


/****************************************************************************
 *
 * Instruction Decode Routines
 *
 ****************************************************************************/

/* _dbg_check_arm_condcode
 *		Check ARM conditional execution code
 *		On entry:
 *			R4: Opcode of instruction to be executed
 *			R5[3:0]: CPSR condition codes
 *		On exit:
 *			R0: will_execute (boolean)
 *          R1-R3: Destroyed
 */

_dbg_check_arm_condcode:
	mov		r0, #TRUE								/* Default will_execute value */
	mov		r3, r4, lsr #28							/* convert opcode's condition code to index (0-F) */
	ldr		r2, =debug_armCondCodeTable
	ldrb	r1, [r2, r3]							/* Get condition code mask */
/*
 * The following check is unnecessary as it is covered by the _dbg_cond_simple_checks checking algorithm
 	teq		r1, #0
	beq		_dbg_check_arm_condcode_exit
*/
	teq		r1, #0xFF
	bne		_dbg_cond_simple_checks


/*
 * Complex Checks:
 *		We assume that CHKNV and CHKC are mutually exclusive.
 *		In addition, it is possible for CHKNV, CHKC and CHKZ to
 *		be cleared, in which case it'll return True (default)
 *
 *
 * will_execute = TRUE [default condition]
 * If (CHKNV) set
 *		// Only N/V, and Z flags are involved
 *		NEQV_Flag = (N == V)
 *		will_execute = (NEQV_Flag == NEQV_Mask)
 *
 * If (CHKC) set
 *		// Only C and Z flags are involved
 *		will_execute = (C_Flag == CSet_Mask)
 *
 * If (CHKZ) set
 *		z_match = (Z_Flag == ZSet_Mask)
 *		If (AND bit set)
 *			will_execute = will_execute && z_match
 *		else
 *			will_execute = will_execute || z_match
 *
 */
_dbg_cond_complex_checks:
	sub		r3, r3, #COMPLEX_CONDCODE_START			/* Convert complex condition code in R3 to new index (0-3) */
	ldr		r2, =debug_armComplexCCTable
	ldrb	r1, [r2, r3]							/* Get complex condition code bitmap in R1 */

_cond_check_nv:
	tst		r1, #COMPLEX_CONDCODE_CHKNV_MASK
	beq		_cond_check_c							/* CHECKNV not set, so skip */
	ands	r2, r5, #(COMPLEX_CONDCODE_NFLAG | COMPLEX_CONDCODE_VFLAG)	/* Is (N == V == 0)? */
	teqne	r2, #(COMPLEX_CONDCODE_NFLAG | COMPLEX_CONDCODE_VFLAG)		/* No, Is (N == V == 1)? */

	moveq	r2, #COMPLEX_CONDCODE_NEQV_MASK			/* EQ: Either (N == V == 0) or (N == V == 1), set R2: COMPLEX_CONDCODE_NEQV_MASK */
	movne	r2, #0									/* NE: N != V, clear R2 */
	and		r3, r1, #COMPLEX_CONDCODE_NEQV_MASK		/* R3: Extract NEQV Mask Value */
	teq		r2, r3									/* Does N/V Condition match NEQV Mask value? */
	movne	r0, #FALSE								/* No, so will_execute = FALSE (for now) */
	b		_cond_check_z

#if 0
	bne		_cond_nnev								/* No, so (N != V) */

	/* EQ: Either (N == V == 0) or (N == V == 1) */
_cond_neqv:
	tst		r1, #COMPLEX_CONDCODE_NEQV_MASK			/* Is (N == V) mask set? */
	moveq	r0, #FALSE								/* No, so will_execute = FALSE (for now) */
	b		_cond_check_z

	/* Else, N != V */
_cond_nnev:
	tst		r1, #COMPLEX_CONDCODE_NEQV_MASK			/* Is (N == V) mask set? */
	movne	r0, #FALSE								/* Yes, so will_execute = FALSE (for now) */
	b		_cond_check_z
#endif

_cond_check_c:
	tst		r1, #COMPLEX_CONDCODE_CHKC_MASK
	beq		_cond_check_z							/* CHECKC not set, so skip */

	/* Use R2 to store C Flag, R3 to store CSet Mask */
	and		r2, r5, #COMPLEX_CONDCODE_CFLAG			/* r2 = C flag */
	and		r3, r1, #COMPLEX_CONDCODE_CSET_MASK		/* r3 = CSet mask */
	teq		r2, r3									/* Does C flag == CSet mask */
	movne	r0, #FALSE								/* No, so C flag failed match */

_cond_check_z:
	tst		r1, #COMPLEX_CONDCODE_CHKZ_MASK
	beq		_dbg_check_arm_condcode_exit			/* No additional checks needed, exit */

	/* Use R2 to store Z Flag, R3 to store ZSet Mask */
	and		r2, r5, #COMPLEX_CONDCODE_ZFLAG			/* r2 = Z flag */
	and		r3, r1, #COMPLEX_CONDCODE_ZSET_MASK		/* r3 = ZSet mask */
	teq		r2, r3									/* Does Z flag == ZSet mask */
	moveq	r3, #TRUE								/* Zero, so z flag matched */
	movne	r3, #FALSE								/* Non-zero, so z flag failed match */

_cond_andor:
	tst		r1, #COMPLEX_CONDCODE_ANDOR_MASK		/* Is ANDOR mask set? */
	andne	r0, r0, r3								/* Yes, so AND with will_execute */
	orreq	r0, r0, r3								/* No, so OR with will_execute */
	b		_dbg_check_arm_condcode_exit			/* Return will_execute (R0) */

/*
 * Simple Checks:
 *		We take advantage of the fact that only 1 bit would be set
 *		in the bitmask, by generating the corresponding actual
 *		CondSet[7:4], CondClr[3:0] value for comparison.
 *
 * will_execute = TRUE [default condition, equivalent to 0x00 (AL) ]
 * Generate CondSetClr[7:0] from CPSR[3:0]
 * will_execute = ((CondSetClr & BitMask) == BitMask)
 *
 */
_dbg_cond_simple_checks:
	eor		r2, r5, #NIBBLE0						/* R2: CondClr[3:0] = Invert CPSR[3:0] */
	orr		r2, r2, r5, lsl #4						/* R2: CondSet[7:4] | CondClr[3:0] */
	and		r2, r2, r1								/* R2: CondSetClr[7:0] & Bitmask */
	teq		r2, r1									/* ((cond_code & SetBitMask) == SetBitMask)? */
	movne	r0, #FALSE								/* Not equal, check failed */

_dbg_check_arm_condcode_exit:
	bx		lr										/* Return to caller */

/* _arm_rmshifted_val
 *      Calculate value of Shifted Rm (operand)
 *      On entry:
 *          R0[11:0]: Shifted Rm operand
 *      On exit:
 *          R0: value of Shifted Rm
 *          R1, R2, R3: destroyed
 */
_arm_rmshifted_val:
    stmfd   sp!, {lr}
    ldr     r3, =(NIBBLE2|BYTE0)
    and     r3, r0, r3                      /* 12 bit Shifted Register operand, copied to R3 */
    and     r2, r3, #NIBBLE0                /* Register Rn Enum in R2 */
    _regenum2index  r2, r2                  /* Convert Enum into Index in R2 */
    _getdbgregisterfromindex r2, r0         /* Retrieve Register Rn contents from Index (R2) into R0 */

    tst     r3, #0x10                       /* B4: Immediate (0) or Register (1) shift count */
    /* check bitshift op */
    and     r3, r3, #0x60                   /* shift type */
    mov     r3, r3, lsr #5                  /* convert into shift type jumptable index */
    bne     _arm_get_reg_shift              /* Flags set previously via TST r3 (B4) */
_arm_calc_const_shift:
    movs    r1, r3, lsr #7                  /* Immediate shift count, 5 bit unsigned value in R1 */
    bne     _arm_calc_shifted_rm_val        /* Non-zero shift count, process normally */
    /* Must check for RRX == ROR #0 */
    teq     r3, #0x3                        /* ROR == 0x3 */
    addeq   r3, r3, #1
    b       _arm_calc_shifted_rm_val

_arm_get_reg_shift:
    mov     r2, r3, lsr #8                  /* Register-based shift count, 4 bit register enum in R2 */
    _regenum2index  r2, r2                  /* Convert Enum into Index in R2 */
    _getdbgregisterfromindex r2, r1         /* Retrieve Register value (shift count) from Index (R2) into R1 */

_arm_calc_shifted_rm_val:
    _dbg_jumpTableHandler   debug_regShiftJumpTable, r2, r3     /* Calculate RmShifted value from R0: Rn Register val, R1: Shift/Rotate val */
    ldmfd   sp!, {pc}

/* Rm Shifted Shift Type Jump Table Routines
 * On entry:
 *      R0: Register Rm
 *      R1: Shift/Rotate Amount
 * On exit:
 *      R0: RmShifted result
 *      R1: destroyed
 *
 */
_reg_lsl:
    lsl     r0, r0, r1
    bx      lr

_reg_lsr:
    lsr     r0, r0, r1
    bx      lr

_reg_asr:
    asr     r0, r0, r1
    bx      lr

_reg_ror:
    ror     r0, r0, r1
    bx      lr

_reg_rrx:
    _getdbgregister DBGSTACK_USERCPSR_INDEX, r1	/* Retrieve CPSR contents into R1 */
    ands    r1, r1, #CPSR_CFLAG            		/* Keep C Flag */
    movne   r1, #0x80000000                 	/* Set B31 if C Flag set */
    lsr     r0, r0, #1                      	/* Rm >> 1 */
    orr     r0, r0, r1                      	/* Put C flag into B31 */
    bx      lr


/* _arm_data_instr_handler
 *      ARM Data Processing Instruction with Rd == R15
 *      On entry:
 *          R4: Opcode of instruction to be executed
 *          R5[3:0]: CPSR condition codes
 *          R6: Default Following Instruction Address (PC+4)
 *      On exit:
 *          R0: following instruction address
 *          R1-R7: Destroyed
 */
_arm_data_instr_handler:
    stmfd   sp!, {lr}
    ldr     r1, =ARM_DATA_INSTR_MASK
    and     r3, r4, r1                      /* Keep base instruction Opcode in R3 */
    ldr     r1, =ARM_DATA_INSTR_MSRMRS
    teq     r3, r1                          /* Check for MSR / MRS instruction */

_arm_is_msr_mrs_instr:
    moveq   r0, r6                          /* Copy default next instruciton address to R0 */
    beq     _exit_arm_data_instr_handler    /* Return default next instruction address */

    /* Not MSR / MRS, so process normally */
_arm_check_operand2_type:
    tst     r4, #ARM_DATA_INSTR_IMMREG      /* Check for Immediate (1) or Register (0) Operand 2 */
    beq     _arm_op2_is_reg

_arm_op2_is_imm:
    and     r1, r4, #BYTE0                  /* 8 bit unsigned constant in R1 */
    and     r2, r4, #NIBBLE2                /* (rotate count / 2) in R2[11:8] */
    lsr     r2, r2, #7                      /* actual rotate count in R2[4:0] */
    ror     r1, r1, r2                      /* Rotated constant in R1 */
    b       _arm_get_operand1_val

_arm_op2_is_reg:
    ldr     r1, =(NIBBLE2|BYTE0)
    and     r0, r4, r1                      /* 12 bit register operand in R1 */
    bl      _arm_rmshifted_val              /* R0 contains the Rm shifted val */
    mov     r1, r0                          /* R1: Operand2 val */

_arm_get_operand1_val:
	bl		_dbg_data_instr_retrieve_op1val	/* R0: Register Rn (Operand1) val */

_arm_calc_data_instr_val:
    and     r3, r4, #ARM_DATA_INSTR_NORMAL  /* Mask Instruction Opcode into R3[24:21] */
    lsr     r3, r3, #21                     /* Shift Data Processing Opcode into R3[3:0] */
    /* Calculate data instruction value from R0: Register Rn (Operand1) val, R1: Operand2 val, R5[3:0]: CPSR, R6: Default Next Instr Addr */
    _dbg_jumpTableHandler   debug_dataInstrJumpTable, r2, r3    /* Next Instruction Address in R0 */
_exit_arm_data_instr_handler:
    ldmfd   sp!, {pc}

/* _dbg_data_instr_retrieve_op1val
 * Retrieve Data Instruction Operand 1 value
 * On entry:
 *      R4: Opcode of instruction to be executed
 *      R5[3:0]: CPSR condition codes
 *      R6: Default Next Instruction Address (PC+4)
 * On exit:
 *      R0: Register Rn (Operand 1) value
 *      R2, R3: Destroyed
 *
 */
_dbg_data_instr_retrieve_op1val:
    and     r3, r4, #NIBBLE4             	/* Store Rn (Operand1) Register Enum into R3[19:16] */
    lsr     r3, r3, #16                     /* Shift into R3[3:0] */
    _regenum2index  r3, r2                  /* Convert Enum into Index in R2 */
    _getdbgregisterfromindex r2, r0         /* Retrieve Register contents from Index (R2) into R0 */
    teq     r3, #REG_PC                     /* Check if it is PC relative */
    addeq   r0, r0, #8                      /* R0: Register Rn (Operand1) val; adjust for PC relative (+8) */
	bx		lr

/* Data Processing Instruction Jump Table Routines
 * On entry:
 *      R0: Register Rn (Operand 1) value
 *      R1: Operand 2 value
 *      R5[3:0]: CPSR condition codes
 *      R6: Default Next Instruction Address (PC+4)
 * On exit:
 *      R0: Calculated result
 *      R1, R2, R3: Destroyed
 *
 */
_opcode_and:
    and r0, r0, r1
    bx  lr

_opcode_eor:
    eor r0, r0, r1
    bx  lr

_opcode_sub:
    sub r0, r0, r1
    bx  lr

_opcode_rsb:
    rsb r0, r0, r1
    bx  lr

_opcode_add:
    add r0, r0, r1
    bx  lr

_opcode_adc:
    /* Op1 + Op2 + C */
    tst     r5, #(CPSR_CFLAG>> 28)          /* R5[3:0] is shifted CPSR value: Test C Flag */
    add     r0, r0, r1
    addne   r0, r0, #1                      /* Add C if set */
    bx      lr

_opcode_sbc:
    /* Op1 - Op2 + C - 1 */
    tst     r5, #(CPSR_CFLAG>> 28)          /* R5[3:0] is shifted CPSR value: Test C Flag */
    sub     r0, r0, r1
    subeq   r0, r0, #1                      /* If C clear, subtract 1, else (C - 1) = 0 */
    bx      lr

_opcode_rsc:
    /* Op2 - Op1 + C - 1 */
    tst     r5, #(CPSR_CFLAG>> 28)          /* R5[3:0] is shifted CPSR value: Test C Flag */
    rsb     r0, r0, r1
    subeq   r0, r0, #1                      /* If C clear, subtract 1, else (C - 1) = 0 */
    bx  lr

_opcode_tst:
_opcode_teq:
_opcode_cmp:
_opcode_cmn:
    mov r0, r6          /* Next Instruction Address is not modified */
    bx  lr

_opcode_orr:
    orr r0, r0, r1
    bx  lr

_opcode_mov:
    mov r0, r1          /* Operand 1 is ignored */
    bx  lr

_opcode_bic:
    bic r0, r0, r1
    bx  lr

_opcode_mvn:
    mvn r0, r1          /* Operand 1 is ignored */
    bx  lr

/* _arm_bx_blx_handler
 *      BX or BLX Rm Handler. Note v4t does not have BLX instr
 *      On entry:
 *          R4: Opcode of instruction to be executed
 *          R5[3:0]: CPSR condition codes
 *          R6: Default Following Instruction Address (PC+4)
 *      On exit:
 *          R0: following instruction address (B0 set to indicate Thumb mode)
 *          R1,R2: destroyed
 */
_arm_bx_blx_handler:
    stmfd   sp!, {lr}
    and     r2, r4, #NIBBLE0                /* Register Rn Enum in R2 */
    _regenum2index  r2, r1                  /* Convert Enum into Index in R1 */
    _getdbgregisterfromindex r1, r0         /* Retrieve Register contents from Index (R1) into R0 */
	teq		r2, #REG_PC
	addeq	r0, r0, #8						/* Adjust PC relative register value (for BX PC) */
	/* Here, the register value would have B0 set to indicate switch to Thumb mode  */
    ldmfd   sp!, {pc}

/* _arm_ldr_pc_handler
 *      LDR with Rd = PC
 *      On entry:
 *          R4: Opcode of instruction to be executed
 *          R5[3:0]: CPSR condition codes
 *          R6: Default Following Instruction Address (PC+4)
 *      On exit:
 *          R0: following instruction address
 *          R1, R2, R3, R4, R5: destroyed
 */

_arm_ldr_pc_handler:
    stmfd   sp!, {lr}

	mov		r1, #0							/* R1: Post-Indexed Offset (cleared) */
    tst     r4, #ARM_LDR_INSTR_PREPOST      /* Pre (1) or Post (0) Indexed */
	beq		_get_rn_val						/* If Post-Indexed, just use Rn directly */

    /* Pre-Indexed */
    ldr     r0, =(NIBBLE2|BYTE0)
    and     r0, r4, r0                      /* R0: 12 bit Immediate value or Shifted Reg operand */
    tst     r4, #ARM_LDR_INSTR_REGIMM       /* Register (1) or Immediate (0) */
    beq     _calc_ldr_pc_offset             /* Immediate value is already in R0 */

_get_shiftedreg_val:
    bl      _arm_rmshifted_val              /* Convert Rm shifted operand in R0 into value in R0 */

_calc_ldr_pc_offset:
	mov		r1, r0							/* Keep Offset in R1 */
_get_rn_val:
	bl		_dbg_data_instr_retrieve_op1val	/* R0: Register Rn (Operand1) val */
_calc_op1val_with_offset:
    tst     r4, #ARM_LDR_INSTR_UPDOWN       /* Add (1) or Subtract (0) */
    addne   r0, r0, r1                      /* If Add, R0 = Rn + Offset */
    subeq   r0, r0, r1                      /* If Sub, R0 = Rn - Offset */

_get_ldr_pc_val_from_mem:
	ldr		r0, [r0]						/* Retrieve value from Memory at address given in R0 */
    ldmfd   sp!, {pc}

/* _arm_ldm_pc_handler
 *      LDM {pc}
 *      On entry:
 *          R4: Opcode of instruction to be executed
 *          R5[3:0]: CPSR condition codes
 *          R6: Default Following Instruction Address (PC+4)
 *      On exit:
 *          R0: following instruction address
 *          R2, R3: destroyed
 *
 *      Note: The algorithm from eCos arm_stub.c does not deal with the Pre/Post-Indexed addressing (P) bit.
 *			The algorithm here loads different content using LDM based on the value of the P bit.
 */
_arm_ldm_pc_handler:
    stmfd   sp!, {lr}
	bl		_dbg_data_instr_retrieve_op1val	/* R0: Register Rn (Operand1) val */

_arm_get_regcount:
    mov     r2, #0                          /* Initialize reg_count (R2) to 0 */
    mov     r3, r4, lsl #16					/* Keep HLFWORD0 containing vector bits in R3[31:16] */
	/* This shortens the checking to a max of 16 iterations, since the PC bit should be set */
1:  movs    r3, r3, lsl #1                  /* count number of '1' bits */
    addcs   r2, r2, #1                      /* increment reg_count (R2) if C Flag set */
    bne     1b                              /* continue until vector is empty */

	/* Pre-Incr:  Rn += reg_count x 4
	 * Post-Incr: Rn += (reg_count - 1) x 4
	 * Pre-Decr:  Rn -= 4
	 * Post-Decr: Rn
	 */

_arm_check_updown_offset:
    tst     r4, #ARM_LDM_INSTR_UPDOWN       /* Check Up (1) or Down (0) */
    beq		_arm_check_prepost_decr

_arm_check_prepost_incr:
    tst     r4, #ARM_LDM_INSTR_PREPOST      /* Check Pre (1) or Post (0) */
    subeq	r2, r2, #1						/* Post-Incr: Decrement reg_count in R2 */
    add		r0, r0, r2, lsl #2              /* Increment Offset: Rn (R0) += reg_count (R2) x 4 */
	b		_get_ldm_pc_val_from_mem

_arm_check_prepost_decr:
    tst     r4, #ARM_LDM_INSTR_PREPOST      /* Check Pre (1) or Post (0) */
	/* Post-Decr: Rn unchanged */
    subne   r0, r0, #4                      /* Pre-Decr: Rn (R0) -= 4 */

_get_ldm_pc_val_from_mem:
    ldr     r0, [r0]                        /* Retrieve stack content for new PC value */
    ldmfd   sp!, {pc}


/* _arm_b_bl_blx_handler
 *      B, BL or BLX <offset>. Note v4t does not have BLX instr
 *      On entry:
 *          R4: Opcode of instruction to be executed
 *          R5[3:0]: CPSR condition codes
 *          R6: Default Following Instruction Address (PC+4)
 *      On exit:
 *          R0: following instruction address
 *          R1: destroyed
 */

_arm_b_bl_blx_handler:
    stmfd   sp!, {lr}

_arm_b_bl_blx_get_offset:
    and     r0, r4, #(BYTE2|BYTE1|BYTE0)    /* Encoded Branch offset in R4[23:0] */
    lsl		r0, r0, #(32-24)				/* Shift to R0[31:8] */
    asr		r0, r0, #(32-26)				/* Actual Signed offset = Encode Offset x 4 in R0[25:0] */
	add		r1, r6, #4						/* R1: (PC+4) + 4 */
    add     r0, r0, r1                      /* Calculate Branch Target Address R0: (PC+8) + signed offset */

#ifndef __ARM6OR7__
	/* armv5t or later, has BLX support */
	and		r1, r4, #ARM_BLX_INSTR_MASK		/* Mask out Condition Code and Opcode */
	teq		r1, #ARM_BLX_INSTR_BLX			/* Look for BLX */
	bne		_exit_arm_b_bl_blx_handler		/* No, it is a B/BL instruction */
	tst		r4, #ARM_BLX_INSTR_HBIT			/* H bit for Thumb Halfword Address */
	orrne	r0, r0, #0x02					/* Set Halfword Address R0[1] */
    orr   	r0, r0, #BKPT_STATE_THUMB_FLAG  /* Set R0[0] since BLX instr used to switch to Thumb mode */
#endif

_exit_arm_b_bl_blx_handler:
    ldmfd   sp!, {pc}

/* _arm_coproc_swi_handler
 *      SVC (SWI) or Coprocessor instruction
 *      On entry:
 *          R4: Opcode of instruction to be executed
 *          R5[3:0]: CPSR condition codes
 *          R6: Default Following Instruction Address (PC+4)
 *      On exit:
 *          R0: following instruction address
 */

_arm_coproc_swi_handler:
    and     r0, r4, #ARM_SWI_INSTR_MASK
    teq     r0, #ARM_SWI_INSTR_VAL          /* SVC (SWI) instruction */

    ldreq   r0, =SVC_VECTOR                 /* SWI: Return SVC Vector Address */
    movne   r0, r6                          /* CoProc: Use default Following Instruction Address */
_exit_arm_coproc_swi_handler:
	bx	    lr

/* _thumb_bx_blx_handler
 *      BX or BLX Handler. Note: Link (L:b7) is not checked in the mask; armv4t does not support BLX.
 *      On entry:
 *          R4: Opcode of instruction to be executed
 *          R5[3:0]: CPSR condition codes
 *          R6: Default Following Instruction Address (PC+2)
 *      On exit:
 *          R0: following instruction address (B0 set to indicate Thumb mode)
 *          R1, R2: destroyed
 */
_thumb_bx_blx_handler:
    stmfd   sp!, {lr}
    and     r2, r4, #THUMB_BLX_INSTR_REG_RNMASK	/* Register Rn Enum in R2[6:3] (Hi-Reg indicated by B6) */
    mov		r2, r2, lsr #3					/* Shift Rn Enum to R2[3:0] */
    _regenum2index  r2, r1                  /* Convert Enum into Index in R1 */
    _getdbgregisterfromindex r1, r0         /* Retrieve Register contents from Index (R1) into R0 */
	teq		r2, #REG_PC
	addeq	r0, r0, #4						/* Adjust PC relative register value (for BX PC) */
	/* Here, the register value would have R0[0] set to indicate switch to Thumb mode  */
    ldmfd   sp!, {pc}

/* _thumb_poppc_handler
 *     PUSH/POP, specifically POP {Rlist,PC}
 *      On entry:
 *          R4: Opcode of instruction to be executed
 *          R5[3:0]: CPSR condition codes
 *          R6: Default Following Instruction Address (PC+2)
 *      On exit:
 *          R0: following instruction address (B0 set to indicate Thumb mode)
 *          R1-R3: destroyed
 */
_thumb_poppc_handler:
    stmfd   sp!, {lr}

_thumb_get_SP_val:
    _getdbgregister DBGSTACK_USERSP_INDEX, r1	/* Retrieve SP contents into R1 */

_thumb_get_regcount:
    mov     r3, r4, lsl #24					/* Keep BYTE0 containing vector bits in R3[31:24] */
	/* POP is equivalent to LDMFD. Load PC is encoded in b8,
	 * the 8-bit vector is for Lo registers.
	 * This shortens the checking to a max of 8 iterations
	 */
1:  movs    r3, r3, lsl #1                  /* count number of '1' bits */
	addcs	r1, r1, #4						/* Walk the stack to locate the PUSHed LR (POP PC) value */
    bne     1b                              /* continue until vector is empty */
	ldr		r0, [r1]						/* Retrieve new PC value */
#if 0
	/* PC Value should have B0 set */
    orr     r0, r0, #BKPT_STATE_THUMB_FLAG	/* Force R0[0] since it is used to indicates Thumb mode */
#endif
    ldmfd   sp!, {pc}

/* _thumb_bcond_swi_handler
 *      B<cond> or SWI (SVC)
 *      On entry:
 *          R4: Opcode of instruction to be executed
 *          R5[3:0]: CPSR condition codes
 *          R6: Default Following Instruction Address (PC+2)
 *      On exit:
 *          R0: following instruction address (B0 set to indicate Thumb mode)
 *          R1-R3: destroyed
 */
_thumb_bcond_swi_handler:
    stmfd   sp!, {lr}
    and     r2, r4, #THUMB_BCOND_SWI_INSTR_CONDMASK		/* Keep Condition Code R2[11:8] */
    teq     r2, #THUMB_BCOND_SWI_INSTR_SWI              /* SVC (SWI) instruction */
_thumb_swi_instr:
    ldreq   r0, =SVC_VECTOR                 /* Return SVC Vector Address */
    beq     _exit_thumb_bcond_swi_handler	/* Switch to ARM mode for SVC */
_thum_bcond_unused_instr:
    teq		r2, #THUMB_BCOND_SWI_COND_UNUSED
    moveq   r0, r6                          /* False (don't execute), so use Default Following Instruction Address */
    beq     _exit_thumb_bcond_instr

_thumb_bcond_instr:
	stmfd	sp!, {r4}						/* Preserve Opcode in R4 */
	lsl		r4, r2, #(32-12)		        /* Shift condition code in R2[11:8] to R0[31:28] to match ARM cond-code format */
	bl		_dbg_check_arm_condcode	        /* Use ARM condition code checking routine to test (R4, R6 unchanged) */
	ldmfd	sp!, {r4}						/* Restore Opcode in R4 */
    teq     r0, #FALSE
    moveq   r0, r6                          /* False (don't execute), so use Default Following Instruction Address */
    beq     _exit_thumb_bcond_instr

_thumb_calc_bcond_offset:
    lsl		r0, r4, #(32-8)					/* Shift 8-bit offset in R4[7:0] to R0[31:24] */
    asr		r0, r0, #(32-9)					/* Convert into 9-bit signed offset in R0[8:0] */
    add		r0, r6, r0						/* PC+2 + signed offset */
    add		r0, r0, #2						/* PC+4 + signed offset */
_exit_thumb_bcond_instr:
    orr     r0, r0, #BKPT_STATE_THUMB_FLAG  /* Set R0[0] since it is used to indicates Thumb mode */
_exit_thumb_bcond_swi_handler:
    ldmfd   sp!, {pc}

/* _thumb_b_handler
 *      B
 *      On entry:
 *          R4: Opcode of instruction to be executed
 *          R5[3:0]: CPSR condition codes
 *          R6: Default Following Instruction Address (PC+2)
 *      On exit:
 *          R0: following instruction address (B0 set to indicate Thumb mode)
 *          R1: destroyed
 *		Note: The signed offset is 12-bits (encoded value x 2)
 */
_thumb_b_handler:
    stmfd   sp!, {lr}
    lsl		r0, r4, #(32-11)		/* Shift 11-bit offset in R4[10:0] to R0[31:21] */
    asr		r0, r0, #(32-12)		/* Convert into 12-bit signed offset in R0[11:0] */
    add		r0, r6, r0				/* PC+2 + signed offset */
    add		r0, r0, #2				/* PC+4 + signed offset */
    orr     r0, r0, #BKPT_STATE_THUMB_FLAG /* Set R0[0] since it is used to indicates Thumb mode */
    ldmfd   sp!, {pc}

/* _thumb_long_bl_blx_handler
 *      Long BL or BLX (4 bytes) Note: b11 (H) indicates 1st or 2nd instr; armv4t does not support BLX.
 *      On entry:
 *          R4: Opcode of instruction to be executed
 *          R5[3:0]: CPSR condition codes
 *          R6: Default Following Instruction Address (PC+2)
 *      On exit:
 *          R0: following instruction address (B0 set to indicate Thumb mode)
 *          R1, R2, R3: destroyed
 *			R6: Subseqent Instruction Address (PC+4) if first instruction is valid, else unchanged (PC+2)
 *		Note: The BL instruction (0xFxxx) should be in pairs (Dual 16-bit instructions).
 *		The first instruction should have (H=0) to indicate the upper 11 bits of the encoded offset
 *		The second instruction should have (H=1) to indicate the lower 11 bits of the encoded offset
 *		The signed offset is 23 bits (encoded value x 2)
 *
 *		Note2: The BLX instruction (0xExxx) encodes the first instruciton using BL (0xFxxx) with H=0,
 *		while the second instruction has a different opcode value (0xExxx), with H=1.
 *		BLX is only used to switch to an ARM target.
 */
_thumb_long_bl_blx_handler:
    stmfd   sp!, {lr}
_thumb_check_1st_bl_blx_instruction:
	tst		r4, #THUMB_BLX_INSTR_IMM_HBIT	/* Check H bit */
	bne		_return_default_thumb_following_instr	/* H=1 as first instruction shouldn't happen */
_thumb_check_2nd_bl_blx_instruction:
    ldrh	r3, [r6]						/* Get second instruction in pair at PC+2 into R3 */
	add		r6, r6, #2						/* Skip to Subsequent Instruction (PC+4) */
	tst		r3, #THUMB_BLX_INSTR_IMM_HBIT	/* Check H bit */
	beq		_return_default_thumb_following_instr	/* H=0 as second instruction shouldn't happen */

_thumb_concat_branch_offset:
    lsl		r0, r4, #(32-11)			/* Shift first instruction 11-bit offset in R4[10:0] to R0[31:21] */
    asr		r0, r0, #(32-23)			/* Convert into 12-bit signed offset in R0[22:12] */
    lsl		r2, r3, #(32-11)			/* Shift second instruction 11-bit offset in R3[10:0] to R2[31:21] */
    lsr		r2, r2, #(32-12)			/* Convert into 12-bit unsigned offset in R2[11:0] */
	orr		r0, r0, r2					/* Combine offsets */
    add		r0, r6, r0					/* PC+4 + signed offset */

_thumb_check_bl_blx_pair:
    and		r3, r3, #THUMB_BLX_INSTR_IMM_MASK	/* Keep second instruction opcode in R3 */
	teq		r3, #THUMB_BLX_INSTR_IMM_BL			/* Look for BL */
	beq		_flag_thumb_instr_addr				/* Return BL target address in R0 */

#ifndef __ARM6OR7__
	/* v5t or higher architecture */
	teq		r3, #THUMB_BLX_INSTR_IMM_BLX		/* Look for BLX */
	biceq	r0, r0, #0x03						/* Match, Force ARM address */
    beq		_exit_thumb_long_bl_blx_handler
#endif

_return_default_thumb_following_instr:
	/* FIXME: This assumes that once the 4-byte sequence check fails, it will return PC+4,
	 * regardless of whether the second instruction is a valid BL/BLX instruction or not.
	 */
	mov		r0, r6								/* Return default Following/Subsequent Instruction Address  */
_flag_thumb_instr_addr:
    orr		r0, r0, #BKPT_STATE_THUMB_FLAG      /* Set R0[0] since it is used to indicates Thumb mode */

_exit_thumb_long_bl_blx_handler:
    ldmfd   sp!, {pc}