--- a/python/st-flash.py	Sat Jan 05 19:07:14 2013 +0100
+++ b/python/st-flash.py	Sun Jan 06 18:13:17 2013 +0100
@@ -1,22 +1,64 @@
 #!/usr/bin/python
 
-import argparse
-from stlink import STLink
+import argparse, sys
+import stlink 
 
 def hex2int(s):
-   return int(s, 16)
+   if s.startswith('0x'):
+      s = s[2:]
+      return int(s, 16)
+   return int(s)
 
 parser = argparse.ArgumentParser(description='ST-link flash utility')
-#parser.add_argument('filename', type=str)
-#parser.add_argument('address', type=hex2int)
+subparsers = parser.add_subparsers(title='commands', 
+         description='possible commands', dest='command')
+
+readparser = subparsers.add_parser('read', help='read flash contents')
+readparser.add_argument('filename', type=argparse.FileType(mode='wb'))
+readparser.add_argument('address', type=hex2int)
+readparser.add_argument('size', type=hex2int)
+
+writeparser = subparsers.add_parser('write', help='write flash contents')
+writeparser.add_argument('filename', type=argparse.FileType(mode='rb'))
+writeparser.add_argument('address', type=hex2int)
+
+eraseparser = subparsers.add_parser('erase', help='erase flash contents')
 
 args = parser.parse_args()
 print(args)
+if not args.command:
+   parser.print_usage()
+   sys.exit(1)
 
-stl = STLink()
-print(stl)
-stl.open()
-if stl.CurrentMode == STLink.DFU_MODE:
-   pass
+# In any command case, open a device:
+stl = stlink.STLink()
+try:
+   stl.open()
+except stlink.STLinkException as e:
+   print(e)
+   sys.exit(2)
+
+# Enter the right mode:
+if stl.CurrentMode == stlink.DFU_MODE:
+   stl.exitDfuMode()
+
+if stl.CurrentMode != stlink.DEBUG_MODE:
+   stl.enterSwdMode()
 
+if stl.ChipId != 0x10016413:
+   print('Only working on stm32f4discovery board for now.')
+   sys.exit(2)
 
+if args.command == 'read':
+   print('Reading!')
+elif args.command == 'write':
+   content = args.filename.read()
+   print('Write flash :)', len(content))
+   stl.writeFlash(args.address, content)
+elif args.command == 'erase':
+   print('erase')
+else:
+   print('unknown command', args.command)
+   
+stl.exitDebugMode()
+