// MTP.h - MTP Responder
#include "MTP.h"
#include "myFlash.h"
#include "usb.h"
#include <string.h>
struct MTPHeader {
union{
byte lenL; // 0
word lenW;
uint32_t lenD;
byte lenB[4];
};
byte typeL,typeH; // 4
byte opL,opH; // 6
uint32_t transid; // 8
};
struct{
MTPHeader h;
uint32_t params[3]; // 12
}bulkcs;
struct event_t{
byte codeL;
byte codeH;
uint32_t sessid;
uint32_t transid;
uint32_t params[3];
void send(byte,byte);
}event;
void event_t::send(byte code,byte param) {
codeL=code;
codeH=0x40;
*(byte*)params = param;
usbSend(this,sizeof*this,3);
usbFlush();
}
static byte utf16len(const char*str) { // Länge für UTF-16 in Words, str!=nullptr
byte r=0;
while (takeUTF8(str)) ++r;
return r;
}
namespace MTP {
char extendedeventdata[8];
devicestatus_t devicestatus={4,0x2001};
static void outc(const byte*p,byte len) {
usbSend(p,len);
}
static void out1(byte b) {
outc(&b,1);
}
static void out2(word w) {
out1(w);
out1(w>>8);
}
static void out4(word w) {
out2(w);
out2(0);
}
static void out0(byte len) {
usbSend(0,len);
}
static void out8(word w) {
out2(w);
out0(6);
}
static void outHeader(word len) {
out4(len+12);
out2(2); // type
outc(&bulkcs.h.opL,6); // operation + transaction ID
}
static void outFlush() {
usbFlush();
}
static void outS(const char*s) {
byte len=utf16len(s)+1;
out1(len);
do out2(takeUTF8(s)); while(--len);
}
static byte lenS(const char*s) { // Bytes für String
return 1+(utf16len(s)+1<<1);
}
static void outDescriptor() {
static const PROGMEM char Model[]="GIF reader";
outHeader(11+10+17+lenS(FP(Model))+2);
out0(11);
out4(3); // Only needed capabilities (Windows 10)
out2(0x100B); // DeleteObject // required otherwise files are deletion protected
out2(0x100C); // SendObjectInfo // required otherwise files cannot be added
out2(0x100D); // SendObject // same as above
out0(17); // lot of garbage
outS(FP(Model)); // sonst erscheint „Tragbares Gerät“
out0(2);
outFlush();
}
static void outStorageIDs() {
outHeader(8);
out4(1); // 1 entry
out4(1); // 1 storage
outFlush();
}
static void outStorageInfo() {
static const PROGMEM char Medium[]="GRM10 card";
outHeader(2+2+2+8+8+4+lenS(FP(Medium))+1);
out2(3); // storage type (fixed RAM)
out2(1); // filesystem type (generic flat)
out2(0); // access capability (read-write)
out8(myFlash::getCapacity()); // max capacity (bytes)
out8(myFlash::getFreeSpace()); // free space (bytes)
out4(myFlash::getFreeObjects()); // free space (objects)
outS(FP(Medium)); // storage descriptor, sonst erscheint „Wechselmedien“
out0(1); // volume identifier
outFlush();
}
static void outObjectHandles() {
byte num = myFlash::GetNumObjects();
outHeader(4+4*num);
out4(num);
for (byte a=1; a<myFlash::FSTART; a++) {
if (myFlash::eer(a)!=0xFF) out4(a);
}
outFlush();
}
static void outObjectInfo(byte handle) {
const myFlash::sec_t*s=myFlash::GetObjectInfo(handle);
outHeader(4+4+4+40+lenS(s->dirent.name)+33+33+1);
out4(1); // Logical Storage ID
out4(0x3000); // undefiniert, Text
out4(pgm_read_word(&s->dirent.sizeL)); // size
out0(40); // Info für Bilder
outS(s->dirent.name);
outS(s->dirent.ctime); // date captured - automatischer Typecast-Operator!
outS(s->dirent.mtime); // date modified - automatischer Typecast-Operator!
out0(1); // keywords
outFlush();
}
static bool GetObject(byte handle) {
byte b=myFlash::eer(handle); // 1. Sektor (muss gültig sein)
if (b<myFlash::FSTART || b>=myFlash::FEND1) return false;
const char*addr=FP((const char*)(b<<myFlash::SEC_SH));
unsigned len=pgm_read_word(addr); // Datei-Länge (sollte der Sektor-Kette entsprechen!)
outHeader(len);
while (len) {
b=myFlash::eer(b); // (Nächster) Datensektor
if (b<myFlash::FSTART || b>=myFlash::FEND1) return false; // Fehler!
addr=FP((const char*)(b<<myFlash::SEC_SH));
unsigned l=1<<myFlash::SEC_SH; if (l>len) l=len;
outc((const byte*)addr,l); // direkt aus Flash
len-=l;
}
outFlush(); // Short-Packet
return true;
}
static myFlash::sec_t secbuf __attribute__((section(".noinit")));
extern "C" void setutf8(word utf16/*,char*&Z*/) __attribute__((naked));
void setutf8(word utf16) {asm(
" cpi r24,0x80 \n"
" cpc r25,r1 \n"
" brcs 1f \n" // 1-Byte-UTF8
" movw r18,r24 \n"
" lsl r18 \n"
" rol r25 \n"
" lsl r18 \n"
" rol r25 \n"
" andi r24,0x3F \n"
" ori r24,0x80 \n"
" ori r25,0xC0 \n"
" cpi r19,0x08 \n"
" brcs 2f \n" // 2-Byte-UTF8
" andi r25,0xBF \n"
" swap r19 \n"
" andi r19,0x0F \n"
" ori r19,0xE0 \n"
" st Z+,r19 \n"
"2: st Z+,r25 \n"
"1: st Z+,r24 \n"
" ret \n"
);}
// Zeiger rückt um 1..3 Byte vor
#if 0
#define dropUTF8(c,s) (__extension__({ \
asm("\tmovw r24,%1\n\trcall setutf8\n":"+z"(s):"r"(c):"r18","r19","r24","r25");\
}))
#else
static void dropUTF8(word c,char*&s) {
asm(" movw r24,%1 \n"
" ld ZL,X+ \n"
" ld ZH,X+ \n"
" rcall setutf8 \n"
" st -X,ZH \n"
" st -X,ZL \n"
::"x"(&s),"r"(c):"r18","r19","r24","r25");}
#endif
static byte usbGetString(char*s,char space) {
byte n;
usbRecv(&n,1);
if (n) for (byte i=0; i<n; i++) {
word w;
usbRecv(&w,2); // UTF-16-Zeichen + Nullterminierung
if (space>0) {
space--;
if (w>=0x80) space--;
if (w>=0x2000) space--;
if (space<=0) *s=0; // nullterminieren wenn Puffer kleiner
else dropUTF8(w,s);
}
}
else if (space) *s=0; // immer nullterminieren
return 1+n+n; // Anzahl von USB abgeholte Bytes
}
// Datei erzeugen: Hier: Verzeichniseintrag initialisieren
static bool SendObjectInfo() {
usbRecv(0,20); // Header, Dateityp ignorieren
secbuf.empty(); // mit 0xFF füllen
usbRecv(&secbuf.dirent.size,4);
usbRecv(0,40); // Krimskrams dazwischen ignorieren
usbGetString(secbuf.dirent.name,sizeof secbuf.dirent.name);
usbGetString(fattime::string,sizeof fattime::string);
secbuf.dirent.ctime();
usbGetString(fattime::string,sizeof fattime::string);
secbuf.dirent.mtime();
usbGetString(nullptr,0); // Rest („keywords“) auslesen
return myFlash::Create(&secbuf);
}
// Datei erzeugen: Jetzt kommen die Daten, und Dateilänge sollte gleich sein.
static bool SendObject() {
word size;
usbRecv(&size,2);
usbRecv(0,10);
size-=12;
while (size) {
byte l=size>sizeof secbuf.data?sizeof secbuf.data:size;
secbuf.empty();
usbRecv(secbuf.data,l);
if (!myFlash::writeNextPage(&secbuf)) return false;
size-=l;
}
return true;
}
static void handleCommand() {
byte p1 = 0;
byte retcode = 1; // Ok
event.transid=bulkcs.h.transid;
if (bulkcs.h.opH!=0x10) retcode=5;
else switch (bulkcs.h.opL) {
case 1: outDescriptor(); break; // GetDescription
case 2: event.sessid=bulkcs.params[0];break; // OpenSession
case 3: event.sessid=0; break; // CloseSession
case 4: outStorageIDs(); break; // GetStorageIDs
case 5: outStorageInfo(); break; // GetStorageInfo
case 6: p1 = myFlash::GetNumObjects();break; // GetNumObjects
case 7: outObjectHandles(); break; // GetObjectHandles
case 8: outObjectInfo(bulkcs.params[0]);break;// GetObjectInfo
case 9: if (!GetObject(bulkcs.params[0])) retcode=21; break; // GetObject
case 11: // DeleteObject
if (!myFlash::DeleteObject(bulkcs.params[0])) retcode = 0x12; // partial deletion
else event.send(3/*object removed*/,bulkcs.params[0]);
break;
case 12: // SendObjectInfo
if (!SendObjectInfo()) retcode=12; // store_full
else bulkcs.params[2] = myFlash::createdDirEnt;
bulkcs.h.lenL = 12 + 3*4; // 3 Parameter
goto ex1;
case 13: // SendObject
if (!SendObject()) retcode=21; // no_valid_objectinfo
bulkcs.h.lenL = 12;
event.send(2/*object added*/,myFlash::createdDirEnt);
break;
default:
retcode = 5; // operation not supported
}
bulkcs.h.lenL = 16; // Standard: Rückgabe 1 Parameter
bulkcs.params[0] = p1;
ex1: // für mehr als 1 Parameter
bulkcs.h.typeL = 3; // Antwort
bulkcs.h.opL = retcode;
bulkcs.h.opH = 0x20;
usbSend(&bulkcs,bulkcs.h.lenL);
usbFlush();
}
// Einzige exportierte Funktion
void pollUSB() {
if (usbRecvPoll()) {
PORTD&=~0x20; // TX
usbRecv(&bulkcs,8);
if (bulkcs.h.typeL==1 // "command", alles andere: STALL (Protokollfehler)
// && bulkcs.h.typeH==0x10
&& bulkcs.h.lenD<=sizeof bulkcs
&& bulkcs.h.lenL>=12) {
usbRecv(&bulkcs.h.transid,bulkcs.h.lenL-8); // Rest des Kommandos abholen
handleCommand();
}else{
// eelog(&bulkcs,8); // Murks!
UEINTX=0; // aufs nächste ganze USB-Paket synchronisieren
}
PORTD|= 0x20;
}
}
}/*namespace*/
| Detected encoding: UTF-8 | 0
|