You can not select more than 25 topics Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.

main.c 13KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414415416417418419420421422423424425426427428429430431432433434435436437438439440441442443444445446447448449450451452453454455456457458459460461462463464465466467468469470471472473474475476477478479480481482483484485486487488489490491492493494495496497498499500501502503504505506507508509510511512513514515516517518519520521522523524525526527528529530531532533534535536537538539540541542543544545546547548549550551552553554555556
  1. /**
  2. * Copyright (c) 2022 Brian Starkey <stark3y@gmail.com>
  3. *
  4. * Based on the Pico W tcp_server example:
  5. * Copyright (c) 2022 Raspberry Pi (Trading) Ltd.
  6. *
  7. * SPDX-License-Identifier: BSD-3-Clause
  8. */
  9. #include <string.h>
  10. #include <stdlib.h>
  11. #include "RP2040.h"
  12. #include "pico/time.h"
  13. #include "hardware/dma.h"
  14. #include "hardware/flash.h"
  15. #include "hardware/structs/dma.h"
  16. #include "hardware/structs/watchdog.h"
  17. #include "hardware/gpio.h"
  18. #include "hardware/resets.h"
  19. #include "hardware/uart.h"
  20. #include "hardware/watchdog.h"
  21. #include "pico/stdlib.h"
  22. #include "pico/cyw43_arch.h"
  23. #include "tcp_comm.h"
  24. extern const char *wifi_ssid;
  25. extern const char *wifi_pass;
  26. #define BOOTLOADER_ENTRY_PIN 15
  27. #define BOOTLOADER_ENTRY_MAGIC 0xb105f00d
  28. #define TCP_PORT 4242
  29. #define IMAGE_HEADER_OFFSET (360 * 1024)
  30. #define WRITE_ADDR_MIN (XIP_BASE + IMAGE_HEADER_OFFSET + FLASH_SECTOR_SIZE)
  31. #define ERASE_ADDR_MIN (XIP_BASE + IMAGE_HEADER_OFFSET)
  32. #define FLASH_ADDR_MAX (XIP_BASE + PICO_FLASH_SIZE_BYTES)
  33. #define CMD_SYNC (('S' << 0) | ('Y' << 8) | ('N' << 16) | ('C' << 24))
  34. #define RSP_SYNC (('W' << 0) | ('O' << 8) | ('T' << 16) | ('A' << 24))
  35. #define CMD_INFO (('I' << 0) | ('N' << 8) | ('F' << 16) | ('O' << 24))
  36. #define CMD_READ (('R' << 0) | ('E' << 8) | ('A' << 16) | ('D' << 24))
  37. #define CMD_CSUM (('C' << 0) | ('S' << 8) | ('U' << 16) | ('M' << 24))
  38. #define CMD_CRC (('C' << 0) | ('R' << 8) | ('C' << 16) | ('C' << 24))
  39. #define CMD_ERASE (('E' << 0) | ('R' << 8) | ('A' << 16) | ('S' << 24))
  40. #define CMD_WRITE (('W' << 0) | ('R' << 8) | ('I' << 16) | ('T' << 24))
  41. #define CMD_SEAL (('S' << 0) | ('E' << 8) | ('A' << 16) | ('L' << 24))
  42. #define CMD_GO (('G' << 0) | ('O' << 8) | ('G' << 16) | ('O' << 24))
  43. #define CMD_REBOOT (('B' << 0) | ('O' << 8) | ('O' << 16) | ('T' << 24))
  44. static uint32_t handle_sync(uint32_t *args_in, uint8_t *data_in, uint32_t *resp_args_out, uint8_t *resp_data_out)
  45. {
  46. return RSP_SYNC;
  47. }
  48. const struct comm_command sync_cmd = {
  49. .opcode = CMD_SYNC,
  50. .nargs = 0,
  51. .resp_nargs = 0,
  52. .size = NULL,
  53. .handle = &handle_sync,
  54. };
  55. static uint32_t size_read(uint32_t *args_in, uint32_t *data_len_out, uint32_t *resp_data_len_out)
  56. {
  57. uint32_t size = args_in[1];
  58. if (size > TCP_COMM_MAX_DATA_LEN) {
  59. return TCP_COMM_RSP_ERR;
  60. }
  61. // TODO: Validate address
  62. *data_len_out = 0;
  63. *resp_data_len_out = size;
  64. return TCP_COMM_RSP_OK;
  65. }
  66. static uint32_t handle_read(uint32_t *args_in, uint8_t *data_in, uint32_t *resp_args_out, uint8_t *resp_data_out)
  67. {
  68. uint32_t addr = args_in[0];
  69. uint32_t size = args_in[1];
  70. memcpy(resp_data_out, (void *)addr, size);
  71. return TCP_COMM_RSP_OK;
  72. }
  73. const struct comm_command read_cmd = {
  74. // READ addr len
  75. // OKOK [data]
  76. .opcode = CMD_READ,
  77. .nargs = 2,
  78. .resp_nargs = 0,
  79. .size = &size_read,
  80. .handle = &handle_read,
  81. };
  82. static uint32_t size_csum(uint32_t *args_in, uint32_t *data_len_out, uint32_t *resp_data_len_out)
  83. {
  84. uint32_t addr = args_in[0];
  85. uint32_t size = args_in[1];
  86. if ((addr & 0x3) || (size & 0x3)) {
  87. // Must be aligned
  88. return TCP_COMM_RSP_ERR;
  89. }
  90. // TODO: Validate address
  91. *data_len_out = 0;
  92. *resp_data_len_out = 0;
  93. return TCP_COMM_RSP_OK;
  94. }
  95. static uint32_t handle_csum(uint32_t *args_in, uint8_t *data_in, uint32_t *resp_args_out, uint8_t *resp_data_out)
  96. {
  97. uint32_t dummy_dest;
  98. uint32_t addr = args_in[0];
  99. uint32_t size = args_in[1];
  100. int channel = dma_claim_unused_channel(true);
  101. dma_channel_config c = dma_channel_get_default_config(channel);
  102. channel_config_set_transfer_data_size(&c, DMA_SIZE_32);
  103. channel_config_set_read_increment(&c, true);
  104. channel_config_set_write_increment(&c, false);
  105. channel_config_set_sniff_enable(&c, true);
  106. dma_hw->sniff_data = 0;
  107. dma_sniffer_enable(channel, 0xf, true);
  108. dma_channel_configure(channel, &c, &dummy_dest, (void *)addr, size / 4, true);
  109. dma_channel_wait_for_finish_blocking(channel);
  110. dma_sniffer_disable();
  111. dma_channel_unclaim(channel);
  112. *resp_args_out = dma_hw->sniff_data;
  113. return TCP_COMM_RSP_OK;
  114. }
  115. struct comm_command csum_cmd = {
  116. // CSUM addr len
  117. // OKOK csum
  118. .opcode = CMD_CSUM,
  119. .nargs = 2,
  120. .resp_nargs = 1,
  121. .size = &size_csum,
  122. .handle = &handle_csum,
  123. };
  124. static uint32_t size_crc(uint32_t *args_in, uint32_t *data_len_out, uint32_t *resp_data_len_out)
  125. {
  126. uint32_t addr = args_in[0];
  127. uint32_t size = args_in[1];
  128. if ((addr & 0x3) || (size & 0x3)) {
  129. // Must be aligned
  130. return TCP_COMM_RSP_ERR;
  131. }
  132. // TODO: Validate address
  133. *data_len_out = 0;
  134. *resp_data_len_out = 0;
  135. return TCP_COMM_RSP_OK;
  136. }
  137. // ptr must be 4-byte aligned and len must be a multiple of 4
  138. static uint32_t calc_crc32(void *ptr, uint32_t len)
  139. {
  140. uint32_t dummy_dest, crc;
  141. int channel = dma_claim_unused_channel(true);
  142. dma_channel_config c = dma_channel_get_default_config(channel);
  143. channel_config_set_transfer_data_size(&c, DMA_SIZE_32);
  144. channel_config_set_read_increment(&c, true);
  145. channel_config_set_write_increment(&c, false);
  146. channel_config_set_sniff_enable(&c, true);
  147. // Seed the CRC calculation
  148. dma_hw->sniff_data = 0xffffffff;
  149. // Mode 1, then bit-reverse the result gives the same result as
  150. // golang's IEEE802.3 implementation
  151. dma_sniffer_enable(channel, 0x1, true);
  152. dma_hw->sniff_ctrl |= DMA_SNIFF_CTRL_OUT_REV_BITS;
  153. dma_channel_configure(channel, &c, &dummy_dest, ptr, len / 4, true);
  154. dma_channel_wait_for_finish_blocking(channel);
  155. // Read the result before resetting
  156. crc = dma_hw->sniff_data ^ 0xffffffff;
  157. dma_sniffer_disable();
  158. dma_channel_unclaim(channel);
  159. return crc;
  160. }
  161. static uint32_t handle_crc(uint32_t *args_in, uint8_t *data_in, uint32_t *resp_args_out, uint8_t *resp_data_out)
  162. {
  163. uint32_t addr = args_in[0];
  164. uint32_t size = args_in[1];
  165. resp_args_out[0] = calc_crc32((void *)addr, size);
  166. return TCP_COMM_RSP_OK;
  167. }
  168. struct comm_command crc_cmd = {
  169. // CRCC addr len
  170. // OKOK crc
  171. .opcode = CMD_CRC,
  172. .nargs = 2,
  173. .resp_nargs = 1,
  174. .size = &size_crc,
  175. .handle = &handle_crc,
  176. };
  177. static uint32_t handle_erase(uint32_t *args_in, uint8_t *data_in, uint32_t *resp_args_out, uint8_t *resp_data_out)
  178. {
  179. uint32_t addr = args_in[0];
  180. uint32_t size = args_in[1];
  181. if ((addr < ERASE_ADDR_MIN) || (addr + size >= FLASH_ADDR_MAX)) {
  182. // Outside flash
  183. return TCP_COMM_RSP_ERR;
  184. }
  185. if ((addr & (FLASH_SECTOR_SIZE - 1)) || (size & (FLASH_SECTOR_SIZE - 1))) {
  186. // Must be aligned
  187. return TCP_COMM_RSP_ERR;
  188. }
  189. flash_range_erase(addr - XIP_BASE, size);
  190. return TCP_COMM_RSP_OK;
  191. }
  192. struct comm_command erase_cmd = {
  193. // ERAS addr len
  194. // OKOK
  195. .opcode = CMD_ERASE,
  196. .nargs = 2,
  197. .resp_nargs = 0,
  198. .size = NULL,
  199. .handle = &handle_erase,
  200. };
  201. static uint32_t size_write(uint32_t *args_in, uint32_t *data_len_out, uint32_t *resp_data_len_out)
  202. {
  203. uint32_t addr = args_in[0];
  204. uint32_t size = args_in[1];
  205. if ((addr < WRITE_ADDR_MIN) || (addr + size >= FLASH_ADDR_MAX)) {
  206. // Outside flash
  207. return TCP_COMM_RSP_ERR;
  208. }
  209. if ((addr & (FLASH_PAGE_SIZE - 1)) || (size & (FLASH_PAGE_SIZE -1))) {
  210. // Must be aligned
  211. return TCP_COMM_RSP_ERR;
  212. }
  213. if (size > TCP_COMM_MAX_DATA_LEN) {
  214. return TCP_COMM_RSP_ERR;
  215. }
  216. // TODO: Validate address
  217. *data_len_out = size;
  218. *resp_data_len_out = 0;
  219. return TCP_COMM_RSP_OK;
  220. }
  221. static uint32_t handle_write(uint32_t *args_in, uint8_t *data_in, uint32_t *resp_args_out, uint8_t *resp_data_out)
  222. {
  223. uint32_t addr = args_in[0];
  224. uint32_t size = args_in[1];
  225. flash_range_program(addr - XIP_BASE, data_in, size);
  226. resp_args_out[0] = calc_crc32((void *)addr, size);
  227. return TCP_COMM_RSP_OK;
  228. }
  229. struct comm_command write_cmd = {
  230. // WRIT addr len [data]
  231. // OKOK crc
  232. .opcode = CMD_WRITE,
  233. .nargs = 2,
  234. .resp_nargs = 1,
  235. .size = &size_write,
  236. .handle = &handle_write,
  237. };
  238. struct image_header {
  239. uint32_t vtor;
  240. uint32_t size;
  241. uint32_t crc;
  242. uint8_t pad[FLASH_PAGE_SIZE - (3 * 4)];
  243. };
  244. static_assert(sizeof(struct image_header) == FLASH_PAGE_SIZE, "image_header must be FLASH_PAGE_SIZE bytes");
  245. static bool image_header_ok(struct image_header *hdr)
  246. {
  247. uint32_t *vtor = (uint32_t *)hdr->vtor;
  248. uint32_t calc = calc_crc32((void *)hdr->vtor, hdr->size);
  249. // CRC has to match
  250. if (calc != hdr->crc) {
  251. return false;
  252. }
  253. // Stack pointer needs to be in RAM
  254. if (vtor[0] < SRAM_BASE) {
  255. return false;
  256. }
  257. // Reset vector should be in the image, and thumb (bit 0 set)
  258. if ((vtor[1] < hdr->vtor) || (vtor[1] > hdr->vtor + hdr->size) || !(vtor[1] & 1)) {
  259. return false;
  260. }
  261. // Looks OK.
  262. return true;
  263. }
  264. static uint32_t handle_seal(uint32_t *args_in, uint8_t *data_in, uint32_t *resp_args_out, uint8_t *resp_data_out)
  265. {
  266. struct image_header hdr = {
  267. .vtor = args_in[0],
  268. .size = args_in[1],
  269. .crc = args_in[2],
  270. };
  271. if ((hdr.vtor & 0xff) || (hdr.size & 0x3)) {
  272. // Must be aligned
  273. return TCP_COMM_RSP_ERR;
  274. }
  275. if (!image_header_ok(&hdr)) {
  276. return TCP_COMM_RSP_ERR;
  277. }
  278. flash_range_erase(IMAGE_HEADER_OFFSET, FLASH_SECTOR_SIZE);
  279. flash_range_program(IMAGE_HEADER_OFFSET, (const uint8_t *)&hdr, sizeof(hdr));
  280. struct image_header *check = (struct image_header *)(XIP_BASE + IMAGE_HEADER_OFFSET);
  281. if (memcmp(&hdr, check, sizeof(hdr))) {
  282. return TCP_COMM_RSP_ERR;
  283. }
  284. return TCP_COMM_RSP_OK;
  285. }
  286. struct comm_command seal_cmd = {
  287. // SEAL vtor len crc
  288. // OKOK
  289. .opcode = CMD_SEAL,
  290. .nargs = 3,
  291. .resp_nargs = 0,
  292. .size = NULL,
  293. .handle = &handle_seal,
  294. };
  295. static void disable_interrupts(void)
  296. {
  297. SysTick->CTRL &= ~1;
  298. NVIC->ICER[0] = 0xFFFFFFFF;
  299. NVIC->ICPR[0] = 0xFFFFFFFF;
  300. }
  301. static void reset_peripherals(void)
  302. {
  303. reset_block(~(
  304. RESETS_RESET_IO_QSPI_BITS |
  305. RESETS_RESET_PADS_QSPI_BITS |
  306. RESETS_RESET_SYSCFG_BITS |
  307. RESETS_RESET_PLL_SYS_BITS
  308. ));
  309. }
  310. static void jump_to_vtor(uint32_t vtor)
  311. {
  312. // Derived from the Leaf Labs Cortex-M3 bootloader.
  313. // Copyright (c) 2010 LeafLabs LLC.
  314. // Modified 2021 Brian Starkey <stark3y@gmail.com>
  315. // Originally under The MIT License
  316. uint32_t reset_vector = *(volatile uint32_t *)(vtor + 0x04);
  317. SCB->VTOR = (volatile uint32_t)(vtor);
  318. asm volatile("msr msp, %0"::"g"
  319. (*(volatile uint32_t *)vtor));
  320. asm volatile("bx %0"::"r" (reset_vector));
  321. }
  322. static uint32_t handle_go(uint32_t *args_in, uint8_t *data_in, uint32_t *resp_args_out, uint8_t *resp_data_out)
  323. {
  324. disable_interrupts();
  325. reset_peripherals();
  326. jump_to_vtor(args_in[0]);
  327. while(1);
  328. return TCP_COMM_RSP_ERR;
  329. }
  330. struct comm_command go_cmd = {
  331. // GOGO vtor
  332. // NO RESPONSE
  333. .opcode = CMD_GO,
  334. .nargs = 1,
  335. .resp_nargs = 0,
  336. .size = NULL,
  337. .handle = &handle_go,
  338. };
  339. static uint32_t handle_info(uint32_t *args_in, uint8_t *data_in, uint32_t *resp_args_out, uint8_t *resp_data_out)
  340. {
  341. resp_args_out[0] = WRITE_ADDR_MIN;
  342. resp_args_out[1] = (XIP_BASE + PICO_FLASH_SIZE_BYTES) - WRITE_ADDR_MIN;
  343. resp_args_out[2] = FLASH_SECTOR_SIZE;
  344. resp_args_out[3] = FLASH_PAGE_SIZE;
  345. resp_args_out[4] = TCP_COMM_MAX_DATA_LEN;
  346. return TCP_COMM_RSP_OK;
  347. }
  348. const struct comm_command info_cmd = {
  349. // INFO
  350. // OKOK flash_start flash_size erase_size write_size max_data_len
  351. .opcode = CMD_INFO,
  352. .nargs = 0,
  353. .resp_nargs = 5,
  354. .size = NULL,
  355. .handle = &handle_info,
  356. };
  357. static void do_reboot(bool to_bootloader)
  358. {
  359. hw_clear_bits(&watchdog_hw->ctrl, WATCHDOG_CTRL_ENABLE_BITS);
  360. if (to_bootloader) {
  361. watchdog_hw->scratch[5] = BOOTLOADER_ENTRY_MAGIC;
  362. watchdog_hw->scratch[6] = ~BOOTLOADER_ENTRY_MAGIC;
  363. } else {
  364. watchdog_hw->scratch[5] = 0;
  365. watchdog_hw->scratch[6] = 0;
  366. }
  367. watchdog_reboot(0, 0, 0);
  368. while (1) {
  369. tight_loop_contents();
  370. asm("");
  371. }
  372. }
  373. static uint32_t size_reboot(uint32_t *args_in, uint32_t *data_len_out, uint32_t *resp_data_len_out)
  374. {
  375. *data_len_out = 0;
  376. *resp_data_len_out = 0;
  377. return TCP_COMM_RSP_OK;
  378. }
  379. static uint32_t handle_reboot(uint32_t *args_in, uint8_t *data_in, uint32_t *resp_args_out, uint8_t *resp_data_out)
  380. {
  381. // Will never return
  382. do_reboot(args_in[0]);
  383. return TCP_COMM_RSP_ERR;
  384. }
  385. struct comm_command reboot_cmd = {
  386. // BOOT to_bootloader
  387. // NO RESPONSE
  388. .opcode = CMD_REBOOT,
  389. .nargs = 1,
  390. .resp_nargs = 0,
  391. .size = &size_reboot,
  392. .handle = &handle_reboot,
  393. };
  394. int main()
  395. {
  396. stdio_init_all();
  397. sleep_ms(1000);
  398. if (cyw43_arch_init()) {
  399. printf("failed to initialise\n");
  400. return 1;
  401. }
  402. cyw43_arch_enable_sta_mode();
  403. printf("Connecting to WiFi...\n");
  404. if (cyw43_arch_wifi_connect_timeout_ms(wifi_ssid, wifi_pass, CYW43_AUTH_WPA2_AES_PSK, 30000)) {
  405. printf("failed to connect.\n");
  406. return 1;
  407. } else {
  408. printf("Connected.\n");
  409. }
  410. const struct comm_command *cmds[] = {
  411. &sync_cmd,
  412. &read_cmd,
  413. &csum_cmd,
  414. &crc_cmd,
  415. &erase_cmd,
  416. &write_cmd,
  417. //&seal_cmd,
  418. //&go_cmd,
  419. &info_cmd,
  420. //&reboot_cmd,
  421. };
  422. struct tcp_comm_ctx *tcp = tcp_comm_new(cmds, sizeof(cmds) / sizeof(cmds[0]), CMD_SYNC);
  423. for ( ; ; ) {
  424. err_t err = tcp_comm_listen(tcp, TCP_PORT);
  425. if (err != ERR_OK) {
  426. printf("Failed to start server: %d\n", err);
  427. sleep_ms(1000);
  428. continue;
  429. }
  430. while (!tcp_comm_server_done(tcp)) {
  431. cyw43_arch_poll();
  432. sleep_ms(1);
  433. }
  434. }
  435. cyw43_arch_deinit();
  436. return 0;
  437. }